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); }