AGVCGPS2DNode::AGVCGPS2DNode()
{
ros::NodeHandle private_node_handle("~");
// Read in GPS topic to subscribe to.
std::string gps_topic("fix");
if (!private_node_handle.getParam("gps_fix_topic", gps_topic))
{
ROS_WARN("[AGVC GPS 2D Node] : <gps_fix_topic> not specified, defaulting to %s", gps_topic.c_str());
}
else
{
ROS_INFO("[AGVC GPS 2D Node] : <gps_fix_topic> set to %s", gps_topic.c_str());
}
// Subscribe to the topic.
gps_sub_ = node_handle_.subscribe(gps_topic, 20, &AGVCGPS2DNode::navSatFixCallback, this);
// Read in GPS topic to publish.
std::string gps_2d_topic("fix_2d");
if (!private_node_handle.getParam("gps_fix_2d_topic", gps_2d_topic))
{
ROS_WARN("[AGVC GPS 2D Node] : <gps_fix_2d_topic> not specified, defaulting to %s", gps_2d_topic.c_str());
}
else
{
ROS_INFO("[AGVC GPS 2D Node] : <gps_fix_2d_topic> set to %s", gps_2d_topic.c_str());
}
// Advertise topic.
gps_2d_pub_ = node_handle_.advertise<sensor_msgs::NavSatFix>(gps_2d_topic, 20);
}
surf::surf(ros::NodeHandle *_n){
first =true;
ros::NodeHandle private_node_handle("~");
private_node_handle.param("rate", rate_, int(10));
//subscribers
featureMap_sub_ = _n->subscribe("/saliency/features_Hou_Maps", 1, &surf::featureMapCallback, this);
//publishers
db_pub_ = _n->advertise<sscrovers_pmslam_common::featureUpdateArray>("SAL_db", 1);
};
void LocalPlannerROS::initialize(std::string name, tf::TransformListener * tf, costmap_2d::Costmap2DROS * costmap_ros)
{
ros::NodeHandle private_node_handle("~/" + name);
local_planner_.parseParams(private_node_handle);
costmap_ros_ = costmap_ros;
costmap_ros_->getRobotPose(current_pose_);
local_planner_util_.initialize(tf, costmap_ros_->getCostmap(), costmap_ros_->getGlobalFrameID());
local_planner_.initialise(local_planner_util_, costmap_ros_);
}
FileSeqPubCore::FileSeqPubCore(ros::NodeHandle *_n) :
it_(*_n)
{
// Initialise node parameters from launch file or command line.
// Use a private node handle so that multiple instances of the node can be run simultaneously
// while using different parameters.
ros::NodeHandle private_node_handle("~");
private_node_handle.param("rate", rate_, double(1));
private_node_handle.param("pub_image_topic_name", pub_image_topic_name_, string("cam_image"));
private_node_handle.param("pub_odom_topic_name", pub_odom_topic_name_, string("odom"));
private_node_handle.param("loop", loop_f_, bool(false));
private_node_handle.param("one_frame", one_frame_, int(-1));
string _tmppath = ros::package::getPath("sscrovers_file_sequence_publisher");
//add '/' wjen missed at the end of path
private_node_handle.param("path_to_images", path_to_images_, string(_tmppath + "/input/"));
if (*path_to_images_.rbegin() != '/') //last element get by reverse_begin iterator
path_to_images_.push_back('/');
private_node_handle.param("image_file_name_prefix", image_file_name_prefix_, string("frame_orig_"));
private_node_handle.param("start_no", start_no_, int(1000000));
//private_node_handle.param("start_no", start_no_, int(0));
private_node_handle.param("image_extension", image_extension_, string(".jpg"));
//add '.' when missed in extension
if (image_extension_[0] != '.')
image_extension_.insert(0, ".");
private_node_handle.param("path_to_trajectory", path_to_poses_, string(_tmppath + "/input/"));
//add '/' wjen missed at the end of path
if (*path_to_poses_.rbegin() != '/') //last element get by reverse_begin iterator
path_to_poses_.push_back('/');
private_node_handle.param("trajectory_file_name", trajectory_file_name_, string("trajectory.fli"));
image_pub_ = it_.advertise(pub_image_topic_name_.c_str(), 0);
odom_pub_ = _n->advertise<nav_msgs::Odometry>(pub_odom_topic_name_.c_str(), 0);
cv_img_ptr_.reset(new cv_bridge::CvImage);
step_ = -1;
loadTrajectory();
}
AGVCGPSOriginNode::AGVCGPSOriginNode() : have_origin_(false)
{
ros::NodeHandle private_node_handle("~");
// Read in GPS topic to subscribe to.
std::string gps_global_topic("fix");
if (!private_node_handle.getParam("gps_global_topic", gps_global_topic))
{
ROS_WARN("[AGVC GPS Origin Node] : <gps_global_topic> not specified, defaulting to %s", gps_global_topic.c_str());
}
else
{
ROS_INFO("[AGVC GPS Origin Node] : <gps_global_topic> set to %s", gps_global_topic.c_str());
}
// Subscribe to the topic.
gps_global_sub_ = node_handle_.subscribe(gps_global_topic, 20, &AGVCGPSOriginNode::gpsCallback, this);
// Read in GPS topic to publish.
std::string gps_local_topic("fix_origin");
if (!private_node_handle.getParam("gps_local_topic", gps_local_topic))
{
ROS_WARN("[AGVC GPS Origin Node] : <gps_local_topic> not specified, defaulting to %s", gps_local_topic.c_str());
}
else
{
ROS_INFO("[AGVC GPS Origin Node] : <gps_local_topic> set to %s", gps_local_topic.c_str());
}
// Advertise the topic.
gps_local_pub_ = node_handle_.advertise<nav_msgs::Odometry>(gps_local_topic, 20);
// Read in GPS origin service name.
std::string gps_origin_service_name("fix");
if (!private_node_handle.getParam("gps_origin_service_name", gps_origin_service_name))
{
ROS_WARN("[AGVC GPS Origin Node] : <gps_origin_service_name> not specified, defaulting to %s", gps_origin_service_name.c_str());
}
else
{
ROS_INFO("[AGVC GPS Origin Node] : <gps_origin_service_name> set to %s", gps_origin_service_name.c_str());
}
// Advertise service to convert GPS coordinates.
gps_origin_server_ = node_handle_.advertiseService(gps_origin_service_name, &AGVCGPSOriginNode::gpsServiceCallback, this);
}
void Run()
{
std::string image_raw_topic_str;
std::string image_obj_topic_str;
ros::NodeHandle private_node_handle("~");//to receive args
if (private_node_handle.getParam("image_raw_node", image_raw_topic_str))
{
ROS_INFO("Setting image node to %s", image_raw_topic_str.c_str());
}
else
{
ROS_INFO("No image node received, defaulting to /image_raw, you can use _image_raw_node:=YOUR_TOPIC");
image_raw_topic_str = "/image_raw";
}
if (private_node_handle.getParam(ros::this_node::getNamespace() + "/img_obj_node", image_obj_topic_str))
{
ROS_INFO("Setting object node to %s", image_obj_topic_str.c_str());
}
else
{
ROS_INFO("No object node received, defaulting to image_obj_ranged, you can use _img_obj_node:=YOUR_TOPIC");
image_obj_topic_str = "image_obj_ranged";
}
publisher_tracked_objects_ = node_handle_.advertise<cv_tracker_msgs::image_obj_tracked>("image_obj_tracked", 1);
ROS_INFO("Subscribing to... %s", image_raw_topic_str.c_str());
ROS_INFO("Subscribing to... %s", image_obj_topic_str.c_str());
subscriber_image_raw_ = node_handle_.subscribe(image_raw_topic_str, 1, &RosTrackerApp::image_callback, this);
subscriber_image_obj_ = node_handle_.subscribe(image_obj_topic_str, 1, &RosTrackerApp::detections_callback, this);
std::string config_topic("/config");
config_topic += ros::this_node::getNamespace() + "/klt";
//node_handle.subscribe(config_topic, 1, &RosTrackerApp::klt_config_cb, this);
ros::spin();
ROS_INFO("END klt");
}
SVMColorDiscriminator() {
ros::NodeHandle nh;
dynamic_reconfigure::Server<saliency_detector::SVMColorDiscriminatorConfig>::CallbackType cb;
cb = boost::bind(&SVMColorDiscriminator::configCallback, this, _1, _2);
dr_srv.setCallback(cb);
ros::NodeHandle private_node_handle("~");
const std::string MODELFILE="model_file";
std::string model_file;
if(!private_node_handle.getParam(MODELFILE, model_file))
{
ROS_ERROR("No model file specified, exiting.");
ros::requestShutdown();
return;
}
svm = cv::ml::StatModel::load<cv::ml::SVM>(model_file,
"opencv_ml_svm");
if(!svm->isTrained())
{
ROS_ERROR("Loading model file %s failed, exiting", model_file.c_str());
ros::requestShutdown();
return;
}
cv::FileStorage fs(model_file, cv::FileStorage::READ);
fs["mean"] >> mean_;
fs["std"] >> std_;
sub_patch_array =
nh.subscribe("projected_patch_array", 1, &SVMColorDiscriminator::patchArrayCallback, this);
pub_named_points =
nh.advertise<samplereturn_msgs::NamedPointArray>("named_point", 1);
pub_debug_image =
nh.advertise<sensor_msgs::Image>("color_debug_image", 1);
}
TEST(WaypointParser, getWaypoints)
{
ros::NodeHandle private_node_handle("~");
XmlRpc::XmlRpcValue xml_waypoints;
private_node_handle.getParam("valid_waypoints", xml_waypoints);
std::vector<nav_msgs::Odometry> waypoints;
EXPECT_TRUE(param_utils::WaypointParser::getWaypoints(xml_waypoints, waypoints));
EXPECT_FLOAT_EQ(waypoints[0].pose.pose.position.x, 1.0);
EXPECT_FLOAT_EQ(waypoints[0].pose.pose.position.y, -1.0);
EXPECT_FLOAT_EQ(waypoints[1].pose.pose.position.x, 2.0);
EXPECT_FLOAT_EQ(waypoints[1].pose.pose.position.y, -2.0);
private_node_handle.getParam("invalid_waypoints_1", xml_waypoints);
EXPECT_FALSE(param_utils::WaypointParser::getWaypoints(xml_waypoints, waypoints));
private_node_handle.getParam("invalid_waypoints_2", xml_waypoints);
EXPECT_FALSE(param_utils::WaypointParser::getWaypoints(xml_waypoints, waypoints));
}
void Run()
{
//ROS STUFF
ros::NodeHandle private_node_handle("~");//to receive args
//RECEIVE IMAGE TOPIC NAME
std::string image_raw_topic_str;
if (private_node_handle.getParam("image_raw_node", image_raw_topic_str))
{
ROS_INFO("Setting image node to %s", image_raw_topic_str.c_str());
}
else
{
ROS_INFO("No image node received, defaulting to /image_raw, you can use _image_raw_node:=YOUR_TOPIC");
image_raw_topic_str = "/image_raw";
}
std::string network_definition_file;
std::string pretrained_model_file;
if (private_node_handle.getParam("network_definition_file", network_definition_file))
{
ROS_INFO("Network Definition File (Config): %s", network_definition_file.c_str());
}
else
{
ROS_INFO("No Network Definition File was received. Finishing execution.");
return;
}
if (private_node_handle.getParam("pretrained_model_file", pretrained_model_file))
{
ROS_INFO("Pretrained Model File (Weights): %s", pretrained_model_file.c_str());
}
else
{
ROS_INFO("No Pretrained Model File was received. Finishing execution.");
return;
}
if (private_node_handle.getParam("score_threshold", score_threshold_))
{
ROS_INFO("Score Threshold: %f", score_threshold_);
}
if (private_node_handle.getParam("nms_threshold", nms_threshold_))
{
ROS_INFO("NMS Threshold: %f", nms_threshold_);
}
ROS_INFO("Initializing Yolo2 on Darknet...");
yolo_detector_.load(network_definition_file, pretrained_model_file, score_threshold_, nms_threshold_);
ROS_INFO("Initialization complete.");
publisher_car_objects_ = node_handle_.advertise<autoware_msgs::image_obj>("/obj_car/image_obj", 1);
publisher_person_objects_ = node_handle_.advertise<autoware_msgs::image_obj>("/obj_person/image_obj", 1);
ROS_INFO("Subscribing to... %s", image_raw_topic_str.c_str());
subscriber_image_raw_ = node_handle_.subscribe(image_raw_topic_str, 1, &Yolo2DetectorNode::image_callback, this);
std::string config_topic("/config");
config_topic += "/yolo2";
subscriber_yolo_config_ = node_handle_.subscribe(config_topic, 1, &Yolo2DetectorNode::config_cb, this);
ros::spin();
ROS_INFO("END Yolo2");
}
void Run()
{
//ROS STUFF
ros::NodeHandle private_node_handle("~");//to receive args
//RECEIVE IMAGE TOPIC NAME
std::string image_raw_topic_str;
if (private_node_handle.getParam("image_raw_node", image_raw_topic_str))
{
ROS_INFO("Setting image node to %s", image_raw_topic_str.c_str());
}
else
{
ROS_INFO("No image node received, defaulting to /image_raw, you can use _image_raw_node:=YOUR_TOPIC");
image_raw_topic_str = "/image_raw";
}
//RECEIVE CONVNET FILENAMES
std::string network_definition_file;
std::string pretrained_model_file;
if (private_node_handle.getParam("network_definition_file", network_definition_file))
{
ROS_INFO("Network Definition File: %s", network_definition_file.c_str());
}
else
{
ROS_INFO("No Network Definition File was received. Finishing execution.");
return;
}
if (private_node_handle.getParam("pretrained_model_file", pretrained_model_file))
{
ROS_INFO("Pretrained Model File: %s", pretrained_model_file.c_str());
}
else
{
ROS_INFO("No Pretrained Model File was received. Finishing execution.");
return;
}
if (private_node_handle.getParam("use_gpu", use_gpu_))
{
ROS_INFO("GPU Mode: %d", use_gpu_);
}
int gpu_id;
if (private_node_handle.getParam("gpu_device_id", gpu_id ))
{
ROS_INFO("GPU Device ID: %d", gpu_id);
gpu_device_id_ = (unsigned int) gpu_id;
}
detect_classes_.push_back(Rcnn::CAR);
detect_classes_.push_back(Rcnn::PERSON);
detect_classes_.push_back(Rcnn::BUS);
//RCNN STUFF
rcnn_detector_ = new RcnnDetector(network_definition_file, pretrained_model_file, use_gpu_, gpu_device_id_);
if (NULL == rcnn_detector_)
{
ROS_INFO("Error while creating RCNN Object");
return;
}
publisher_car_objects_ = node_handle_.advertise<cv_tracker::image_obj>("/obj_car/image_obj", 1);
publisher_person_objects_ = node_handle_.advertise<cv_tracker::image_obj>("/obj_person/image_obj", 1);
ROS_INFO("Subscribing to... %s", image_raw_topic_str.c_str());
subscriber_image_raw_ = node_handle_.subscribe(image_raw_topic_str, 1, &RosRcnnApp::image_callback, this);
std::string config_topic("/config"); config_topic += ros::this_node::getNamespace() + "/rcnn";
subscriber_rcnn_config_ =node_handle_.subscribe(config_topic, 1, &RosRcnnApp::config_cb, this);
ros::spin();
ROS_INFO("END rcnn");
}
