int main(int argc, char* argv[] ) {
ros::init(argc, argv, "sdf_2d");
ros::NodeHandle nh("sdf_main");
ros::NodeHandle private_nh("~");
bool p_bag_mode;
private_nh.param("p_bag_mode", p_bag_mode, false);
sdfslam::SignedDistanceField sdf;
if (!p_bag_mode)
ros::spin();
else {
bool cont = true;
unsigned int microseconds = 10000;
while (ros::ok && cont) {
cont = sdf.checkTimeout();
ros::spinOnce();
usleep(microseconds);
}
}
ROS_INFO("shutting down sdf slam..");
return 0;
}
void OrientGoal::initialize(std::string name,tf::TransformListener* tf,
costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap,
geometry_msgs::Quaternion* goal_orientation ){
if(!initialized_){
name_ = name;
tf_ = tf;
global_costmap_ = global_costmap;
local_costmap_ = local_costmap;
goal_orientation_ = goal_orientation;
//get some parameters from the parameter server
ros::NodeHandle private_nh("~/" + name_);
ros::NodeHandle blp_nh("~/TrajectoryPlannerROS");
//we'll simulate every degree by default
private_nh.param("sim_granularity", sim_granularity_, 0.017);
private_nh.param("frequency", frequency_, 20.0);
blp_nh.param("acc_lim_th", acc_lim_th_, 1.0);
blp_nh.param("max_rotational_vel", max_rotational_vel_, 1.0);
blp_nh.param("min_in_place_rotational_vel", min_rotational_vel_, 0.4);
blp_nh.param("yaw_goal_tolerance", tolerance_, 0.10);
local_costmap_->getCostmapCopy(costmap_);
world_model_ = new base_local_planner::CostmapModel(costmap_);
initialized_ = true;
}
else{
ROS_ERROR("You should not call initialize twice on this object, doing nothing");
}
}
void BoxDetectionNode::loadParameters()
{
ros::NodeHandle private_nh("~");
#define GOP(key, default_value) getOptionalParameter(private_nh, #key, parameters_.key##_, default_value)
#define GRP(key) getRequiredParameter(private_nh, #key, parameters_.key##_)
GOP(point_cloud_topic, std::string("/camera/depth/points"));
GOP(target_frame_id, std::string("base_link"));
GOP(have_action_server_debug_output, true);
GOP(have_box_detection_debug_output, true);
GOP(box_plane_points_min, 100);
GRP(box_plane_size_min);
GRP(box_plane_size_max);
GOP(detection_timeout, 30.0);
GOP(plane_fitting_distance_threshold, 0.02);
GOP(plane_fitting_max_iterations, 50);
GOP(downsampling_leaf_size, 0.005);
GOP(clusterization_tolerance, 0.02);
GOP(have_plane_publisher, true);
GOP(plane_publishing_rate, 2.0);
GOP(have_collision_object_publisher, true);
GOP(collision_objects_basename, std::string("box"));
GOP(have_visualization_marker_publisher, true);
GOP(visualization_marker_namespace, std::string("tedusar_box_detection"));
#undef GOP
#undef GRP
}
int main(int argc, char** argv){
ros::init(argc, argv, "turtlebot_car");
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
//we get the name of the odometry topic
private_nh.getParam("odometry_topic_name", mOdometry_topic_name);
//we get paramaters that define the pose of the URG
//position
x_laser=0.0; y_laser=0.0; z_laser=0.0;
private_nh.getParam("x_laser", x_laser);private_nh.getParam("y_laser", y_laser);private_nh.getParam("z_laser", z_laser);
//orientation
roll_laser=0.0; pitch_laser=0.0; yaw_laser=0.0;
private_nh.getParam("roll_laser", roll_laser);private_nh.getParam("pitch_laser", pitch_laser);private_nh.getParam("yaw_laser", yaw_laser);
//we define the subscriber to odometry
ros::Subscriber sub = nh.subscribe<nav_msgs::Odometry>(mOdometry_topic_name, 10, &odometryCallback);
std::cout<<"***************[ STARING turtlebot_car_node]***************** "<<std::endl;
ros::spin();
return 0;
};
void DWAPlannerROS::initialize(
std::string name,
tf::TransformListener* tf,
costmap_2d::Costmap2DROS* costmap_ros) {
if (! isInitialized()) {
ros::NodeHandle private_nh("~/" + name);
g_plan_pub_ = private_nh.advertise<nav_msgs::Path>("global_plan", 1);
l_plan_pub_ = private_nh.advertise<nav_msgs::Path>("local_plan", 1);
tf_ = tf;
costmap_ros_ = costmap_ros;
costmap_ros_->getRobotPose(current_pose_);
// make sure to update the costmap we'll use for this cycle
costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap();
planner_util_.initialize(tf, costmap, costmap_ros_->getGlobalFrameID());
//create the actual planner that we'll use.. it'll configure itself from the parameter server
dp_ = boost::shared_ptr<DWAPlanner>(new DWAPlanner(name, &planner_util_));
initialized_ = true;
dsrv_ = new dynamic_reconfigure::Server<DWAPlannerConfig>(private_nh);
dynamic_reconfigure::Server<DWAPlannerConfig>::CallbackType cb = boost::bind(&DWAPlannerROS::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);
}
else{
ROS_WARN("This planner has already been initialized, doing nothing.");
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "waypoints_marker_publisher");
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
//subscribe traffic light
ros::Subscriber light_sub = nh.subscribe("light_color", 10, receiveAutoDetection);
ros::Subscriber light_managed_sub = nh.subscribe("light_color_managed", 10, receiveManualDetection);
//subscribe global waypoints
ros::Subscriber lane_array_sub = nh.subscribe("lane_waypoints_array", 10, laneArrayCallback);
ros::Subscriber traffic_array_sub = nh.subscribe("traffic_waypoints_array", 10, laneArrayCallback);
//subscribe local waypoints
ros::Subscriber temporal_sub = nh.subscribe("temporal_waypoints", 10, temporalCallback);
ros::Subscriber closest_sub = nh.subscribe("closest_waypoint", 10, closestCallback);
//subscribe config
ros::Subscriber config_sub = nh.subscribe("config/lane_stop", 10, configParameter);
g_local_mark_pub = nh.advertise<visualization_msgs::MarkerArray>("local_waypoints_mark", 10, true);
g_global_mark_pub = nh.advertise<visualization_msgs::MarkerArray>("global_waypoints_mark", 10, true);
//initialize path color
_initial_color.b = 1.0;
_initial_color.g = 0.7;
_global_color = _initial_color;
_global_color.a = g_global_alpha;
g_local_color = _initial_color;
g_local_color.a = g_local_alpha;
ros::spin();
}
void JointStatePublisher::init()
{
ros::NodeHandle private_nh("~");
std::string yaw_joint_name, pitch_joint_name;
private_nh.param<std::string>("yaw_joint_name", yaw_joint_name, std::string("sh_yaw_joint"));
private_nh.param<std::string>("pitch_joint_name", pitch_joint_name, std::string("sh_pitch_joint"));
double frequency;
private_nh.param<double>("frequency", frequency, 30.0);
sh_joint_states_.name.resize(2);
sh_joint_states_.position.resize(2);
sh_joint_states_.name[0] = yaw_joint_name;
sh_joint_states_.position[0] = 0.0;
sh_joint_states_.name[1] = pitch_joint_name;
sh_joint_states_.position[1] = 0.0;
joint_state_pub_ = nh_.advertise<sensor_msgs::JointState>("sh_joint_state", 1);
servo_1_state_sub_ = nh_.subscribe("servo_yaw_state", 1, &JointStatePublisher::servo1StateMsgCB, this);
servo_2_state_sub_ = nh_.subscribe("servo_pitch_state", 1, &JointStatePublisher::servo2StateMsgCB, this);
publish_joint_state_timer_ = nh_.createTimer(ros::Duration(1/frequency), &JointStatePublisher::publishJointStateTimerCB, this, false );
}
int main(int argc, char *argv[])
{
#if defined(USE_POSIX_SHARED_MEMORY)
attach_ShareMem();
while (1)
{
CvMemStorage *houghStorage = cvCreateMemStorage(0);
IplImage *frame = getImage_fromSHM();
process_image_common(frame);
cvReleaseImage(frame);
}
detach_ShareMem();
#else
ros::init(argc, argv, "line_ocv");
ros::NodeHandle n;
ros::NodeHandle private_nh("~");
std::string image_topic_name;
private_nh.param<std::string>("image_raw_topic", image_topic_name, "/image_raw");
ROS_INFO("Setting image topic to %s", image_topic_name.c_str());
ros::Subscriber subscriber = n.subscribe(image_topic_name, 1, lane_cannyhough_callback);
image_lane_objects = n.advertise<lane_detector::ImageLaneObjects>("lane_pos_xy", 1);
ros::spin();
#endif
return 0;
}
SteeringDriver::SteeringDriver(int argc, char** argv, std::string node_name) {
// Setup ROS stuff
ros::init(argc, argv, "steering_driver");
ros::NodeHandle private_nh("~");
ros::Rate loop_rate(10);
// Setup Subscriber(s)
twist_subscriber =
private_nh.subscribe("/cmd_vel", 10, &SteeringDriver::twistCallback, this);
// Get Params
SB_getParam(private_nh, "port", port, (std::string) "/dev/ttyACM0");
SB_getParam(private_nh, "max_abs_linear_speed", max_abs_linear_speed, 2.0);
SB_getParam(
private_nh, "max_abs_angular_speed", max_abs_angular_speed, 1.0);
// Ensure that the values given for max absolute speeds are positive
max_abs_linear_speed = fabs(max_abs_linear_speed);
max_abs_angular_speed = fabs(max_abs_angular_speed);
// Setup Arduino stuff
// Get the Arduino port from a ros param
// TODO - Give some indication when we attach/detach from the Steering
// Controller. Could be as simple as checking how long it was since we last
// received a message
// Open the given serial port
arduino.Open(port);
arduino.SetBaudRate(LibSerial::SerialStreamBuf::BAUD_9600);
arduino.SetCharSize(LibSerial::SerialStreamBuf::CHAR_SIZE_8);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "laserscan2costmap");
tf::TransformListener tf_listener;
g_tf_listenerp = &tf_listener;
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
private_nh.param<double>("resolution", g_resolution, 0.1);
private_nh.param<int>("scan_size_x", g_scan_size_x, 1000);
private_nh.param<int>("scan_size_y", g_scan_size_y, 1000);
private_nh.param<int>("map_size_x", g_map_size_x, 500);
private_nh.param<int>("map_size_y", g_map_size_y, 500);
private_nh.param<std::string>("scan_topic", g_scan_topic, "/scan");
private_nh.param<std::string>("sensor_frame", g_sensor_frame, "/velodyne");
ros::Subscriber laserscan_sub = nh.subscribe(g_scan_topic, 1, laserScanCallback);
g_map_pub = nh.advertise<nav_msgs::OccupancyGrid>("/ring_ogm", 1);
ros::spin();
return 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "voxel_grid_filter");
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
private_nh.getParam("points_topic", POINTS_TOPIC);
private_nh.getParam("output_log", _output_log);
if(_output_log == true){
char buffer[80];
std::time_t now = std::time(NULL);
std::tm *pnow = std::localtime(&now);
std::strftime(buffer,80,"%Y%m%d_%H%M%S",pnow);
filename = "voxel_grid_filter_" + std::string(buffer) + ".csv";
ofs.open(filename.c_str(), std::ios::app);
}
// Publishers
filtered_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/filtered_points", 10);
points_downsampler_info_pub = nh.advertise<points_downsampler::PointsDownsamplerInfo>("/points_downsampler_info", 1000);
// Subscribers
ros::Subscriber config_sub = nh.subscribe("config/voxel_grid_filter", 10, config_callback);
ros::Subscriber scan_sub = nh.subscribe(POINTS_TOPIC, 10, scan_callback);
ros::spin();
return 0;
}
WmObjectCapture::WmObjectCapture(ros::NodeHandle private_nh_)
: nh_(),
objectFrameId_("/object"),
cameraFrameId_("/camera_link"),
cameraTopicId_("/camera/depth_registered/points"),
object_(new pcl::PointCloud<pcl::PointXYZRGB>),
object_octree_(new pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB>(128.0f)),
max_z_(0.5),
making_object_(true),
unique_shot_(false)
{
ros::NodeHandle private_nh(private_nh_);
private_nh.param("objectFrameId", objectFrameId_, objectFrameId_);
private_nh.param("cameraFrameId", cameraFrameId_, cameraFrameId_);
private_nh.param("cameraTopicId", cameraTopicId_, cameraTopicId_);
private_nh.param("pointcloud_max_z", max_z_,max_z_);
private_nh.param("make_object", making_object_,making_object_);
private_nh.param("unique_shot", unique_shot_,unique_shot_);
pointCloudSub_ = new message_filters::Subscriber<sensor_msgs::PointCloud2> (nh_, cameraTopicId_, 5);
tfPointCloudSub_ = new tf::MessageFilter<sensor_msgs::PointCloud2> (*pointCloudSub_, tfListener_, cameraFrameId_, 5);
tfPointCloudSub_->registerCallback(boost::bind(&WmObjectCapture::insertCloudCallback, this, _1));
objectService_ = nh_.advertiseService("object", &WmObjectCapture::objectSrv, this);
objectPub_ = nh_.advertise<sensor_msgs::PointCloud2>("/object", 1, true);
object_octree_->setInputCloud(object_);
initMarkers();
}
void StepbackRecovery::initialize(std::string name, tf::TransformListener* tf,
costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap) {
if(!initialized_) {
name_ = name;
tf_ = tf;
global_costmap_ = global_costmap;
local_costmap_ = local_costmap;
//get some parameters from the parameter server
ros::NodeHandle private_nh("~/" + name_);
ros::NodeHandle blp_nh("~/TrajectoryPlannerROS");
//Stepping back 10 cm per default
private_nh.param("stepback_length", stepback_length_, 0.1);
private_nh.param("frequency", frequency_, 10.0);
blp_nh.param("acc_lim_th", acc_lim_th_, 3.2);
blp_nh.param("max_rotational_vel", max_rotational_vel_, 1.0);
blp_nh.param("min_in_place_rotational_vel", min_rotational_vel_, 0.4);
blp_nh.param("yaw_goal_tolerance", tolerance_, 0.10);
world_model_ = new base_local_planner::CostmapModel(*local_costmap_->getCostmap());
initialized_ = true;
}
else {
ROS_ERROR("You should not call initialize twice on this object, doing nothing");
}
}
ScanManipulation::ScanManipulation(ros::Rate &publish_retry_rate) :
publish_retry_rate_(publish_retry_rate)
{
nh_ = ros::NodeHandle("");
laser_sub_ = nh_.subscribe<sensor_msgs::LaserScan>("laser_in", 100,
&ScanManipulation::laserCB, this);
pointcloud_sub_ = nh_.subscribe<sensor_msgs::PointCloud2>("pointcloud_in", 10,
&ScanManipulation::pointcloudCB, this);
laser_pub_ = nh_.advertise<sensor_msgs::LaserScan>("laser_out", 100);
pointcloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("pointcloud_out", 10);
ros::NodeHandle private_nh("~");
reconf_server_.reset(new ReconfigureServer(private_nh));
reconf_server_->setCallback(boost::bind(&ScanManipulation::configCB, this, _1, _2));
private_nh.param("frame_id_new", frame_id_new_, frame_id_new_);
private_nh.param("frame_id_to_replace", frame_id_to_replace_, frame_id_to_replace_);
double msg_delay_in_milliseconds, time_offset_in_milliseconds;
private_nh.param("msg_delay_milliseconds", msg_delay_in_milliseconds, msg_delay_.toSec() * 1000);
private_nh.param("time_offset_milliseconds", time_offset_in_milliseconds, time_offset_.toSec() * 1000);
msg_delay_ = ros::Duration(msg_delay_in_milliseconds/1000);
time_offset_ = ros::Duration(time_offset_in_milliseconds/1000);
if (private_nh.hasParam("publish_retry_rate"))
{
double retry_rate;
private_nh.param("publish_retry_rate", retry_rate, 100.0);
publish_retry_rate_ = ros::Rate(retry_rate);
}
}
AppLoader::AppLoader()
{
ros::NodeHandle private_nh("~");
ros::NodeHandle nh;
rtp_manager_destroy_ = false;
//get app name and type
private_nh.param("app_name", app_name_, std::string("app_demo"));
private_nh.param("app_type", app_type_, std::string("micros_swarm/AppDemo"));
ros::ServiceClient client = nh.serviceClient<app_loader::AppLoad>("app_loader_load_app");
app_loader::AppLoad srv;
srv.request.name = app_name_;
srv.request.type = app_type_;
if (client.call(srv))
{
ROS_INFO("[AppLoader]: App %s loaded successfully.", app_name_.c_str());
}
else
{
ROS_ERROR("[AppLoader]: Failed to load App %s.", app_name_.c_str());
}
//when the rtp manager was destroyed, automatically unload the apps
std::string topic_name = "runtime_core_destroy_" + app_name_;
rtp_manager_destroy_srv_ = nh.advertiseService(topic_name, &AppLoader::rtpManagerDestroyCB, this);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "image_proc");
// Check for common user errors
if (ros::names::remap("camera") != "camera")
{
ROS_WARN("Remapping 'camera' has no effect! Start image_proc in the "
"camera namespace instead.\nExample command-line usage:\n"
"\t$ ROS_NAMESPACE=%s rosrun image_proc image_proc",
ros::names::remap("camera").c_str());
}
if (ros::this_node::getNamespace() == "/")
{
ROS_WARN("Started in the global namespace! This is probably wrong. Start image_proc "
"in the camera namespace.\nExample command-line usage:\n"
"\t$ ROS_NAMESPACE=my_camera rosrun image_proc image_proc");
}
// Shared parameters to be propagated to nodelet private namespaces
ros::NodeHandle private_nh("~");
XmlRpc::XmlRpcValue shared_params;
int queue_size;
if (private_nh.getParam("queue_size", queue_size))
shared_params["queue_size"] = queue_size;
nodelet::Loader manager(false); // Don't bring up the manager ROS API
nodelet::M_string remappings;
nodelet::V_string my_argv;
// Debayer nodelet, image_raw -> image_mono, image_color
std::string debayer_name = ros::this_node::getName() + "_debayer";
manager.load(debayer_name, "image_proc/debayer", remappings, my_argv);
// Rectify nodelet, image_mono -> image_rect
std::string rectify_mono_name = ros::this_node::getName() + "_rectify_mono";
if (shared_params.valid())
ros::param::set(rectify_mono_name, shared_params);
manager.load(rectify_mono_name, "image_proc/rectify", remappings, my_argv);
// Rectify nodelet, image_color -> image_rect_color
// NOTE: Explicitly resolve any global remappings here, so they don't get hidden.
remappings["image_mono"] = ros::names::resolve("image_color");
remappings["image_rect"] = ros::names::resolve("image_rect_color");
std::string rectify_color_name = ros::this_node::getName() + "_rectify_color";
if (shared_params.valid())
ros::param::set(rectify_color_name, shared_params);
manager.load(rectify_color_name, "image_proc/rectify", remappings, my_argv);
// Check for only the original camera topics
ros::V_string topics;
topics.push_back(ros::names::resolve("image_raw"));
topics.push_back(ros::names::resolve("camera_info"));
image_proc::AdvertisementChecker check_inputs(ros::NodeHandle(), ros::this_node::getName());
check_inputs.start(topics, 60.0);
ros::spin();
return 0;
}
NavfnWithCostmap::NavfnWithCostmap(string name, Costmap2DROS* cmap) :
NavfnROS(name, cmap)
{
ros::NodeHandle private_nh("~");
cmap_ = cmap;
make_plan_service_ = private_nh.advertiseService("make_plan", &NavfnWithCostmap::makePlanService, this);
pose_sub_ = private_nh.subscribe<rm::PoseStamped>("goal", 1, &NavfnWithCostmap::poseCallback, this);
}
DWAPlanner::DWAPlanner(std::string name, base_local_planner::LocalPlannerUtil *planner_util) :
planner_util_(planner_util),
obstacle_costs_(planner_util->getCostmap()),
path_costs_(planner_util->getCostmap()),
goal_costs_(planner_util->getCostmap(), 0.0, 0.0, true),
goal_front_costs_(planner_util->getCostmap(), 0.0, 0.0, true),
alignment_costs_(planner_util->getCostmap())
{
ros::NodeHandle private_nh("~/" + name);
goal_front_costs_.setStopOnFailure( false );
alignment_costs_.setStopOnFailure( false );
//Assuming this planner is being run within the navigation stack, we can
//just do an upward search for the frequency at which its being run. This
//also allows the frequency to be overwritten locally.
std::string controller_frequency_param_name;
if(!private_nh.searchParam("controller_frequency", controller_frequency_param_name)) {
sim_period_ = 0.05;
} else {
double controller_frequency = 0;
private_nh.param(controller_frequency_param_name, controller_frequency, 20.0);
if(controller_frequency > 0) {
sim_period_ = 1.0 / controller_frequency;
} else {
ROS_WARN("A controller_frequency less than 0 has been set. Ignoring the parameter, assuming a rate of 20Hz");
sim_period_ = 0.05;
}
}
ROS_INFO("Sim period is set to %.2f", sim_period_);
oscillation_costs_.resetOscillationFlags();
private_nh.param("publish_cost_grid_pc", publish_cost_grid_pc_, false);
map_viz_.initialize(name, boost::bind(&DWAPlanner::getCellCosts, this, _1, _2, _3, _4, _5, _6));
std::string frame_id;
private_nh.param("global_frame_id", frame_id, std::string("odom"));
traj_cloud_.header.frame_id = frame_id;
traj_cloud_pub_.advertise(private_nh, "trajectory_cloud", 1);
// set up all the cost functions that will be applied in order
// (any function returning negative values will abort scoring, so the order can improve performance)
std::vector<base_local_planner::TrajectoryCostFunction*> critics;
critics.push_back(&oscillation_costs_); // discards oscillating motions (assisgns cost -1)
critics.push_back(&obstacle_costs_); // discards trajectories that move into obstacles
critics.push_back(&goal_front_costs_); // prefers trajectories that make the nose go towards (local) nose goal
critics.push_back(&alignment_costs_); // prefers trajectories that keep the robot nose on nose path
critics.push_back(&path_costs_); // prefers trajectories on global path
critics.push_back(&goal_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation
// trajectory generators
std::vector<base_local_planner::TrajectorySampleGenerator*> generator_list;
generator_list.push_back(&generator_);
scored_sampling_planner_ = base_local_planner::SimpleScoredSamplingPlanner(generator_list, critics);
}
int main(int ac, char **av)
{
ros::init(ac, av, ros::this_node::getName());
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
mrover::StereoViewer handler(nh, private_nh);
ros::spin();
return 0;
}
int main(int argc, char* argv[])
{
ros::init(argc, argv, "calibration_publisher");
ros::NodeHandle n;
ros::NodeHandle private_nh("~");
if (!private_nh.getParam("register_lidar2camera_tf", isRegister_tf)) {
isRegister_tf = true;
}
if (!private_nh.getParam("publish_extrinsic_mat", isPublish_extrinsic)) {
isPublish_extrinsic = true; /* publish extrinsic_mat in default */
}
if (!private_nh.getParam("publish_camera_info", isPublish_cameraInfo)) {
isPublish_extrinsic = false; /* doesn't publish camera_info in default */
}
if (argc < 2)
{
std::cout << "Usage: calibration_publisher <calibration-file>." << std::endl;
return -1;
}
cv::FileStorage fs(argv[1], cv::FileStorage::READ);
if (!fs.isOpened())
{
std::cout << "Cannot open " << argv[1] << std::endl;
return -1;
}
fs["CameraExtrinsicMat"] >> CameraExtrinsicMat;
fs["CameraMat"] >> CameraMat;
fs["DistCoeff"] >> DistCoeff;
fs["ImageSize"] >> ImageSize;
ros::Subscriber image_sub;
if (isRegister_tf) {
image_sub = n.subscribe("/image_raw", 10, image_raw_cb);
}
if (isPublish_cameraInfo) {
camera_info_pub = n.advertise<sensor_msgs::CameraInfo>("/camera/camera_info", 10, true);
cameraInfo_sender(CameraMat, DistCoeff, ImageSize);
}
if (isPublish_extrinsic) {
projection_matrix_pub = n.advertise<calibration_camera_lidar::projection_matrix>("/projection_matrix", 10, true);
projectionMatrix_sender(CameraExtrinsicMat);
}
ros::spin();
return 0;
}
int main(int argc, char **argv)
{
ROS_INFO_STREAM("pure pursuit start");
// set up ros
ros::init(argc, argv, "pure_pursuit");
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
// setting params
private_nh.getParam("sim_mode", _sim_mode);
ROS_INFO_STREAM("sim_mode : " << _sim_mode);
private_nh.getParam("linear_interpolate_mode", g_linear_interpolate_mode);
ROS_INFO_STREAM("linear_interpolate_mode : " << g_linear_interpolate_mode);
//publish topic
g_cmd_velocity_publisher = nh.advertise<geometry_msgs::TwistStamped>("twist_raw", 10);
_vis_pub = nh.advertise<visualization_msgs::Marker>("next_waypoint_mark", 0);
_stat_pub = nh.advertise<std_msgs::Bool>("wf_stat", 0);
_target_pub = nh.advertise<visualization_msgs::Marker>("next_target_mark", 0);
_search_pub = nh.advertise<visualization_msgs::Marker>("search_circle_mark", 0);
#ifdef DEBUG
_line_point_pub = nh.advertise<visualization_msgs::Marker>("line_point_mark", 0); //debug tool
#endif
_traj_circle_pub = nh.advertise<visualization_msgs::Marker>("trajectory_circle_mark", 0);
//subscribe topic
ros::Subscriber waypoint_subcscriber = nh.subscribe("final_waypoints", 10, WayPointCallback);
// ros::Subscriber waypoint_subcscriber = nh.subscribe("safety_waypoint", 10, WayPointCallback);
ros::Subscriber odometry_subscriber = nh.subscribe("odom_pose", 10, OdometryPoseCallback);
ros::Subscriber ndt_subscriber = nh.subscribe("control_pose", 10, NDTCallback);
ros::Subscriber est_twist_subscriber = nh.subscribe("estimate_twist", 10, estTwistCallback);
ros::Subscriber config_subscriber = nh.subscribe("config/waypoint_follower", 10, ConfigCallback);
ros::Subscriber zmp_can_subscriber = nh.subscribe("can_info", 10, CanInfoCallback);
geometry_msgs::TwistStamped twist;
ros::Rate loop_rate(LOOP_RATE); // by Hz
while (ros::ok())
{
ros::spinOnce();
//check topic
if (_waypoint_set == false || _pose_set == false)
{
ROS_INFO_STREAM("topic waiting...");
loop_rate.sleep();
continue;
}
doPurePursuit();
loop_rate.sleep();
}
return 0;
}
ExploreAction::ExploreAction( std::string name ) :
node_(),
e_( NULL ),
action_name_( name ),
as_( nh_, name, boost::bind( &ExploreAction::executeCB, this, _1 ), false )
{
ros::NodeHandle private_nh("~");
//e_ = new Explore();
as_.start();
}
void BeaconKFNode::initializeRos( void )
{
ros::NodeHandle private_nh("~");
ros::NodeHandle nh("");
private_nh.param("world_fixed_frame", _world_fixed_frame, std::string("map"));
private_nh.param("odometry_frame", _odometry_frame, std::string("odom"));
private_nh.param("platform_frame", _platform_frame, std::string("platform"));
//setup platform offset position and angle
int platform_index;
std::vector<double> empty_vec;
private_nh.param("platform_index", platform_index, 0);
private_nh.param("platform_x_coords", _platform_x_coords, empty_vec);
private_nh.param("platform_y_coords", _platform_y_coords, empty_vec);
double reference_x;
double reference_y;
private_nh.param("reference_x", reference_x, 1.0);
private_nh.param("reference_y", reference_y, 0.0);
_reference_coords.push_back(reference_x);
_reference_coords.push_back(reference_y);
double platform_angle;
private_nh.param("platform_orientation", platform_angle, 0.0);
_platform_origin.setX(_platform_x_coords[platform_index]);
_platform_origin.setY(_platform_y_coords[platform_index]);
_platform_origin.setZ(0);
platform_angle = platform_angle*(M_PI/180);
platform_angle = platform_angle + atan(_reference_coords[1]/_reference_coords[0]);
_platform_orientation = tf::createQuaternionFromYaw(platform_angle);
//beacon message subscriber callback
_beacon_sub = nh.subscribe( "beacon_pose", 1, &BeaconKFNode::beaconCallback, this);
//define timer rates
double update_period;
double broadcast_period;
private_nh.param("update_period", update_period, 1.0);
private_nh.param("broadcast_period", broadcast_period, 0.05);
//timer for broadcasting the map correction xfrom
_transform_broadcast_timer = private_nh.createTimer(ros::Duration(broadcast_period),
&BeaconKFNode::transformBroadcastCallback,
this);
//timer for updating the EKF
_update_timer = private_nh.createTimer(ros::Duration(update_period),
&BeaconKFNode::filterUpdateCallback,
this);
//set dynamic reconfigure callback
dr_server.setCallback(boost::bind(&BeaconKFNode::configCallback, this, _1, _2));
}
/*
* Constructor
*/
CButCamCast::CButCamCast(const std::string & name,rviz::VisualizationManager * manager)
: Display( name, manager )
, m_bPublish(true)
{
// Create and connect pane
rviz::WindowManagerInterface * wi( manager->getWindowManager() );
if( wi != 0 )
{
// Arm manipulation controls
m_pane = new CControllPane( wi->getParentWindow(), wxT("Screencasts manager"), wi);
if( m_pane != 0 )
{
wi->addPane( "Screencasts manager", m_pane );
wi->showPane( m_pane );
// Connect signal
m_pane->getSigChckBox().connect( boost::bind( &CButCamCast::onPublishStateChanged, this, _1) );
m_pane->getSigSave().connect( boost::bind( &CButCamCast::onSave, this, _1 ) );
}
}else{
std::cerr << "No window manager, no panes :( " << std::endl;
}
// Get node handle
ros::NodeHandle private_nh("/");
// Set parameters
// Get camera screencasting topic name
private_nh.param("rviz_screencast_topic_name", m_camCastPublisherName, CAMERA_SCREENCAST_TOPIC_NAME);
// Get timer period
private_nh.param("publishig_period", m_timerPeriod, DEFAULT_PUBLISHING_PERIOD );
// Get scene node
m_sceneNode = scene_manager_->getRootSceneNode()->createChildSceneNode();
// Create publisher
this->m_camCastPublisher = private_nh.advertise< sensor_msgs::Image >(m_camCastPublisherName, 100, false);
// Create geometry
createGeometry(private_nh);
// Create publishing timer
m_timer = private_nh.createTimer( ros::Duration(m_timerPeriod), boost::bind( &CButCamCast::onTimerPublish, this, _1) );
// Set publishing parameters
m_textureWithRtt->setFrame("/map");
}
NavfnWithLocalCostmap::NavfnWithLocalCostmap(string name, Costmap2DROS* cmap) :
NavfnROS(name, cmap)
{
inscribed_radius_ = 0.0;
circumscribed_radius_ = 0.0;
inflation_radius_ = 0.0;
ros::NodeHandle private_nh("~");
set_costmap_service_ = private_nh.advertiseService("set_costmap", &NavfnWithLocalCostmap::setCostmap, this);
make_plan_service_ = private_nh.advertiseService("make_plan", &NavfnWithLocalCostmap::makePlanService, this);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "twist_gate");
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
TwistGate twist_gate(nh, private_nh);
ros::spin();
return 0;
}
void RuntimeCore::setParameters()
{
bool param_ok = node_handle_.getParam("/publish_robot_id_duration", publish_robot_base_duration_);
if(!param_ok) {
std::cout<<"could not get parameter publish_robot_id_duration! use the default value."<<std::endl;
publish_robot_base_duration_ = 0.1;
}
else {
std::cout<<"publish_robot_id_duration: "<<publish_robot_base_duration_<<std::endl;
}
param_ok = node_handle_.getParam("/publish_swarm_list_duration", publish_swarm_list_duration_);
if(!param_ok) {
std::cout<<"could not get parameter publish_swarm_list_duration! use the default value."<<std::endl;
publish_swarm_list_duration_ = 5.0;
}
else {
std::cout<<"publish_swarm_list_duration: "<<publish_swarm_list_duration_<<std::endl;
}
param_ok = node_handle_.getParam("/default_neighbor_distance", default_neighbor_distance_);
if(!param_ok) {
std::cout<<"could not get parameter default_neighbor_distance! use the default value."<<std::endl;
default_neighbor_distance_ = 50;
}
else {
std::cout<<"default_neighbor_distance: "<<default_neighbor_distance_<<std::endl;
}
param_ok = node_handle_.getParam("/total_robot_numbers", total_robot_numbers_);
if(!param_ok) {
std::cout<<"could not get parameter total_robot_numbers! use the default value."<<std::endl;
total_robot_numbers_ = 1;
}
else {
std::cout<<"total_robot_numbers: "<<total_robot_numbers_<<std::endl;
}
param_ok = node_handle_.getParam("/comm_type", comm_type_);
if(!param_ok) {
std::cout<<"could not get parameter comm_type, use the default ros_comm."<<std::endl;
comm_type_ = "ros_comm/ROSComm";
}
else {
std::cout<<"comm_type: "<<comm_type_<<std::endl;
}
ros::NodeHandle private_nh("~");
private_nh.param("robot_id", robot_id_, 0);
std::cout<<"robot_id: "<<robot_id_<<std::endl;
private_nh.param("worker_num", worker_num_, 4);
std::cout<<"worker_num: "<<worker_num_<<std::endl;
}
KinectV2Joy::KinectV2Joy(){
ros::NodeHandle private_nh("~");
hand_sub = private_nh.subscribe("/kinect_v2/hand_state", 1, &KinectV2Joy::hand_callback, this);
joy_sub = private_nh.subscribe("/hydra_joy", 1, &KinectV2Joy::hydra_callback, this);
tracked_id_sub = private_nh.subscribe("/kinect_v2/surrogate_id", 1, &KinectV2Joy::tracked_id_callback, this);
joy_pub = private_nh.advertise<sensor_msgs::Joy>("/kinect_v2/joy", 1);
private_nh.param("lopen_button", lopen_button, 5);
private_nh.param("lclose_button", lclose_button, 7);
private_nh.param("ropen_button", ropen_button, 13);
private_nh.param("rclose_button", rclose_button, 15);
private_nh.param("rx_joy", rx_joy, 2);
private_nh.param("ry_joy", ry_joy, 3);
private_nh.param("n_buttons", n_buttons, 16);
private_nh.param("n_axes", n_axes, 16);
if( lopen_button > n_buttons
|| lclose_button > n_buttons
|| ropen_button > n_buttons
|| rclose_button > n_buttons
|| lopen_button < 0
|| lclose_button < 0
|| ropen_button < 0
|| rclose_button < 0
)
{
ROS_FATAL("buttons exceed array bounds");
exit(1);
}
if( rx_joy > n_axes
|| ry_joy > n_axes
|| rx_joy < 0
|| ry_joy < 0
)
{
ROS_FATAL("axes exceed array bounds");
exit(1);
}
joy_msg.buttons.resize(n_buttons);
joy_msg.axes.resize(n_axes);
tracked_id = -1;
ros::spin();
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "mode_publisher") ;
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
std::string frame_id;
private_nh.getParam("frame_id", frame_id);
ROS_INFO_STREAM("frame_id : " << frame_id);
std::string topic_name;
private_nh.getParam("topic_name", topic_name);
ROS_INFO_STREAM("topic_name : " << topic_name);
std::string model_path;
private_nh.getParam("model_path", model_path);
ROS_INFO_STREAM("model_path : " << model_path);
ros::Publisher pub = nh.advertise<visualization_msgs::Marker>(topic_name,10, true);
visualization_msgs::Marker marker;
marker.header.frame_id = frame_id;
marker.header.stamp = ros::Time();
marker.ns = topic_name;
marker.id = 0;
marker.action = visualization_msgs::Marker::ADD;
marker.type = visualization_msgs::Marker::MESH_RESOURCE;
marker.mesh_resource = model_path;
marker.mesh_use_embedded_materials = true;
marker.pose.position.x = 0;
marker.pose.position.y = 0;
marker.pose.position.z = 0;
double roll = 90 * (M_PI /180);
double yaw = 180 * (M_PI / 180);
double pitch = 0;
marker.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
marker.color.r = 0.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
marker.color.a = 0.0;
marker.scale.x = 1.0;
marker.scale.y = 1.0;
marker.scale.z = 1.0;
marker.frame_locked = true;
pub.publish(marker);
ros::spin();
return 0;
}
TimeManager::TimeManager(int buffer_size) {
ros::NodeHandle private_nh("~");
private_nh.param("is_points_image", is_points_image_, true);
private_nh.param("is_vscan_image", is_vscan_image_, false);
if (is_vscan_image_ == is_points_image_) {
ROS_ERROR("choose is_points_image or is_vscan_image");
is_points_image_ = true;
is_vscan_image_ = false;
}
time_monitor_pub = nh.advertise<synchronization::time_monitor> ("/times", 10);
image_raw_sub = nh.subscribe("/sync_drivers/image_raw", 10, &TimeManager::image_raw_callback, this);
points_raw_sub = nh.subscribe("/sync_drivers/points_raw", 10, &TimeManager::points_raw_callback, this);
points_image_sub = nh.subscribe("/points_image", 10, &TimeManager::points_image_callback, this);
vscan_points_sub = nh.subscribe("/vscan_points", 10, &TimeManager::vscan_points_callback, this);
vscan_image_sub = nh.subscribe("/vscan_image", 10, &TimeManager::vscan_image_callback, this);
image_obj_sub = nh.subscribe("/image_obj", 10, &TimeManager::image_obj_callback, this);
image_obj_ranged_sub = nh.subscribe("/image_obj_ranged", 10, &TimeManager::image_obj_ranged_callback, this);
image_obj_tracked_sub = nh.subscribe("/image_obj_tracked", 10, &TimeManager::image_obj_tracked_callback, this);
current_pose_sub = nh.subscribe("/current_pose", 10, &TimeManager::current_pose_callback, this);
obj_label_sub = nh.subscribe("/obj_label", 10, &TimeManager::obj_label_callback, this);
cluster_centroids_sub = nh.subscribe("/cluster_centroids", 10, &TimeManager::cluster_centroids_callback, this);
obj_pose_sub = nh.subscribe("/obj_pose_timestamp", 10, &TimeManager::obj_pose_callback, this);
// obj_pose_sub = nh.subscribe("/obj_car/obj_pose", 10, &TimeManager::obj_pose_callback, this);
// sync
sync_image_obj_ranged_sub = nh.subscribe("/sync_ranging/image_obj", 10 , &TimeManager::sync_image_obj_ranged_callback, this);
sync_image_obj_tracked_sub = nh.subscribe("/sync_tracking/image_obj_ranged", 10, &TimeManager::sync_image_obj_tracked_callback, this);
sync_obj_label_sub = nh.subscribe("/sync_reprojection/image_obj_tracked", 10, &TimeManager::sync_obj_label_callback, this);
sync_obj_pose_sub = nh.subscribe("/sync_obj_fusion/obj_label", 10, &TimeManager::sync_obj_pose_callback, this);
// time difference
time_diff_sub = nh.subscribe("/time_difference", 10, &TimeManager::time_diff_callback, this);
image_raw_.resize(buffer_size);
points_raw_.resize(buffer_size);
points_image_.resize(buffer_size);
vscan_points_.resize(buffer_size);
vscan_image_.resize(buffer_size);
image_obj_.resize(buffer_size);
image_obj_ranged_.resize(buffer_size);
image_obj_tracked_.resize(buffer_size);
current_pose_.resize(buffer_size);
obj_label_.resize(buffer_size);
obj_pose_.resize(buffer_size);
cluster_centroids_.resize(buffer_size);
sync_image_obj_ranged_.resize(buffer_size);
sync_image_obj_tracked_.resize(buffer_size);
sync_obj_label_.resize(buffer_size);
sync_obj_pose_.resize(buffer_size);
time_diff_.resize(buffer_size);
}