Esempio n. 1
0
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;
};
Esempio n. 5
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 );
}
Esempio n. 8
0
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;
}
Esempio n. 9
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;
}
Esempio n. 11
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;
}
Esempio n. 12
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");
    }
}
Esempio n. 14
0
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);
    }
Esempio n. 16
0
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;
}
Esempio n. 17
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);
}
Esempio n. 18
0
  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);
  }
Esempio n. 19
0
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;
}
Esempio n. 20
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;

}
Esempio n. 21
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;
}
Esempio n. 22
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));
    
}
Esempio n. 24
0
/*
 *  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");

}
Esempio n. 25
0
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);
}
Esempio n. 26
0
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;
    }
Esempio n. 28
0
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();
}
Esempio n. 29
0
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;
}
Esempio n. 30
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);
}