Пример #1
0
int main(int argc, char **argv) {
    if (argc < 2) {
        printf("\nusage: relay TOPIC [VARIANT_TOPIC]\n\n");
        return 1;
    }

    if (!getTopicBase((argv[1]), publisherTopic)) {
        ROS_ERROR("Failed to extract topic base from [%s]", argv[1]);
        return 1;
    }

    ros::init(argc, argv, publisherTopic+"_relay",
              ros::init_options::AnonymousName);

    if (argc == 2)
        publisherTopic = std::string(argv[1])+"_relay";
    else
        publisherTopic = argv[2];
    subscriberTopic = argv[1];

    nodeHandle.reset(new ros::NodeHandle("~"));

    bool unreliable = false;
    nodeHandle->getParam("unreliable", unreliable);
    nodeHandle->getParam("lazy", lazy);

    if (unreliable)
        subscriberTransportHints.unreliable().reliable();

    subscribe();
    ros::spin();

    return 0;
}
Пример #2
0
    bool OnInit()
    {

      // create our own copy of argv, with regular char*s.
      local_argv_ = new char*[argc];
      for (int i = 0; i < argc; ++i)
      {
        local_argv_[ i ] = strdup( wxString( argv[ i ] ).mb_str() );
      }

      ros::init(argc, local_argv_, "remote_monitor");
      nh_.reset(new ros::NodeHandle);

      //wxInitAllImageHandlers();

      CRemoteFrame* frame = new CRemoteFrame(
        nh_,
        NULL,
        wxID_ANY,
        wxT("Remote Frame"),
        wxDefaultPosition,
        wxSize(700, 350),
        wxDEFAULT_FRAME_STYLE & ~wxRESIZE_BORDER);

      SetTopWindow(frame);
      frame->Show();

      return true;
    }
Пример #3
0
int main(int argc, char** argv) {
    ros::init(argc, argv, "batteryInfo");
    nodeHandle = ros::NodeHandlePtr(new ros::NodeHandle("~"));
    getNamePrefix();
    ROS_INFO("Subscribing to /diagnostics ...");
    ros::Subscriber diagnosticsSub = nodeHandle->subscribe("/diagnostics", 1, diagnosticsCallback);
    ros::spin();
    return 0;
}
Пример #4
0
void callback_crash(const std_msgs::TimeConstPtr& time)
{
    _mute = true;

    revert_applied_readings_since(time->data);
    _timer = _handle->createTimer(ros::Duration(_muting_time), callback_timer, true, true );

    ROS_ERROR("[PoseGenerator::callbackCrash] Crash signal received. Will mute encoder readings for %.2lf seconds", _muting_time);
}
int main(int argc, char** argv)
{
    ros::init(argc, argv, "db_cloud_extraction");
    nh.reset(new ros::NodeHandle);
    ros::NodeHandle pn("~");

    //plane filter
    pn.param<float>("normal_angle", normal_angle, 15.0);
    //filter1
    pn.param<float>("search_radius", search_radius, 0.8);
    pn.param<int>("neighbour_required", neighbour_required, 20);
    //filter2
    pn.param<int>("statistical_knn", statistical_knn, 50);
    pn.param<float>("std_dev_dist", std_dev_dist, 1.0);

    ros::ServiceServer table_srv = nh->advertiseService("db_table_clouds", extract);
    ros::ServiceServer table_srv2 = nh->advertiseService("db_table", extract2);

    ros::spin();

}
Пример #6
0
int main(int argc, char** argv)
{
    ros::init(argc, argv, "centreview");
    n.reset(new ros::NodeHandle);
    ros::NodeHandle pn("~");


    std::string collection;
    std::vector<int> indices;
    pn.param<std::string>("collection",collection, "table_centre_group");
    pn.getParam("specify_index",indices);

    if(collection=="table_centre_group"){

        mongodb_store::MessageStoreProxy table_centre_group(*n, collection);
        std::vector<boost::shared_ptr<geometry_msgs::Polygon> > result_tables;
        table_centre_group.query<geometry_msgs::Polygon>(result_tables);

        VIZ_Points vv(n,"/table_centre_group");
        std::vector<float> color;
        color.push_back(0.0);
        color.push_back(1.0);
        color.push_back(0.0);

        VIZ_Points vv2(n,"/table_centre_group2");
        std::vector<float> color2;
        color2.push_back(0.0);
        color2.push_back(0.0);
        color2.push_back(1.0);

        VIZ_Points vv3(n,"/table_centre_group3");
        std::vector<float> color3;
        color3.push_back(1.0);
        color3.push_back(0.0);
        color3.push_back(0.0);

        while (ros::ok()){
            vv.pub_polygonASpoints(result_tables,0,color);

            vv2.pub_polygonASpoints(result_tables,1,color2);

            vv3.pub_polygonASpoints(result_tables,2,color3);
            sleep(1);
        }
        return 0;
    }

}
int main(int argc, char *argv[])
{
  /*
   * INITIALIZE ROS NODE
   */
  ros::init(argc, argv, "perception_node");
  ros::NodeHandle priv_nh_("~");

  priv_nh_.param<double>("leaf_size", leaf_size_, 0.0f);
//  priv_nh_.param<double>("passThrough_max", passThrough_max_, 1.0f);
//  priv_nh_.param<double>("passThrough_min", passThrough_min_, -1.0f);
//  priv_nh_.param<double>("maxIterations", maxIterations_, 200.0f);
//  priv_nh_.param<double>("distThreshold", distThreshold_, 0.01f);
//  priv_nh_.param<double>("clustTol", clustTol_, 0.01f);
//  priv_nh_.param<double>("clustMax", clustMax_, 10000.0);
//  priv_nh_.param<double>("clustMin", clustMin_, 300.0f);

  nh_.reset(new ros::NodeHandle());
//  ros::ServiceServer server = nh_->advertiseService("filter_cloud", &filterCallback);
  ros::spin();
  return 0;
}
Пример #8
0
int main(int argc, char **argv) {
  if (argc < 2) {
    printf("\nusage: echo TOPIC\n\n");
    return 1;
  }
  
  if (!getTopicBase((argv[1]), subscriberTopic)) {
    ROS_ERROR("Failed to extract topic base from [%s]", argv[1]);
    return 1;
  }
  
  ros::init(argc, argv, subscriberTopic+"_echo",
    ros::init_options::AnonymousName);
  
  subscriberTopic = argv[1];
  
  nodeHandle.reset(new ros::NodeHandle("~"));
 
  subscribe();
  ros::spin();
  
  return 0;
}
Пример #9
0
int main(int argc, char **argv)
{
    ros::init(argc, argv, "pose_generator");

    _handle = ros::NodeHandlePtr(new ros::NodeHandle(""));

    _odom.header.frame_id = "map";
    _x = _y = 0;
    _theta = 0;
    _correct_theta = false;
    _correct_lateral = false;
    _iteration_theta = 0; _iteration_lateral = 0;
    _turn_accum = 0;
    _heading = 0;

    _see_front_plane = false;

    _ringbuffer.reserve(_ringbuffer_max_size);

    ros::Subscriber sub_enc = _handle->subscribe("/arduino/encoders",10,callback_encoders);
    ros::Subscriber sub_turn_angle = _handle->subscribe("/controller/turn/angle",10,callback_turn_angle);
    ros::Subscriber sub_turn_done = _handle->subscribe("/controller/turn/done",10,callback_turn_done);
    ros::Subscriber sub_ir = _handle->subscribe("/perception/ir/distance",10,callback_ir);
    ros::Subscriber sub_planes = _handle->subscribe("/vision/obstacles/planes",10,callback_planes);
    ros::Subscriber sub_crash = _handle->subscribe("/perception/imu/peak", 10, callback_crash);

    _pub_odom = _handle->advertise<nav_msgs::Odometry>("/pose/odometry/",10,(ros::SubscriberStatusCallback)connect_odometry_callback);
    _pub_viz = _handle->advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
    _pub_compass = _handle->advertise<std_msgs::Int8>("/pose/compass", 10, (ros::SubscriberStatusCallback)connect_compass_callback);

    _srv_raycast = _handle->serviceClient<navigation_msgs::Raycast>("/mapping/raycast");

    ros::spin();

    return 0;
}
Пример #10
0
void getNamePrefix() {
    if (!nodeHandle->getParam("batteryDiagnosticsPublisherNode", namePrefix)) {
        ROS_INFO_STREAM("No prefix parameter specified. using: nifti_robot_driver");
        namePrefix = "nifti_robot_driver";
    }
}
Пример #11
0
 TurtleApp(int& argc, char** argv)
   : QApplication(argc, argv)
 {
   ros::init(argc, argv, "turtlesim", ros::init_options::NoSigintHandler);
   nh_.reset(new ros::NodeHandle);
 }
  int init()
  {
    // Use global namespace for node
    node_ = ros::NodeHandlePtr(new ros::NodeHandle());
    // Get tf_prefix from global namespace
    node_->param("tf_prefix", tf_prefix_, std::string(""));

    // Use private namespace for parameters
    pnode_ = ros::NodeHandlePtr(new ros::NodeHandle("~"));
    pnode_->param("publish_rate", publish_rate_, PUBLISH_RATE);

    pnode_->param("fixed_frame_id", fixed_frame_id_, std::string("odom"));
    fixed_frame_id_ = tf::resolve(tf_prefix_, fixed_frame_id_);

    pnode_->param("publish_odom_tf", publish_odom_tf_, true);

    pnode_->param("crio/ip", crio_ip_, std::string("127.0.0.1"));
    pnode_->param("crio/command_port", crio_cmd_port_, std::string("39000"));
    pnode_->param("crio/state_port", crio_state_port_, std::string("39001"));
    pnode_->param("crio/socket_timeout", socket_timeout_, 10);

    VehicleKinematics::Parameters kin_params;
    tfScalar minimum_radius_outer;

    if (!pnode_->getParam("kinematics/frame_id", kin_params.frame_id))
    {
      ROS_WARN_STREAM(pnode_->getNamespace() << "/kinematics/frame_id parameter is not set");
    }
    else
    {
      kin_params.frame_id = tf::resolve(tf_prefix_, kin_params.frame_id);
    }

    if (!pnode_->getParam("kinematics/wheelbase", kin_params.wheelbase_length))
    {
      ROS_FATAL_STREAM(pnode_->getNamespace() << "/kinematics/wheelbase parameter is not set");
      return -1;
    }
    if (!pnode_->getParam("kinematics/track", kin_params.track_width))
    {
      ROS_FATAL_STREAM( pnode_->getNamespace() << "/kinematics/track parameter is not set");
      return -1;
    }
    if (!pnode_->getParam("kinematics/rotation_center", kin_params.rotation_center))
    {
      ROS_WARN_STREAM(
          pnode_->getNamespace()
          << "/kinematics/rotation_center parameter is not set. Using default: wheelbase/2 = "
          << kin_params.wheelbase_length / 2);
      kin_params.rotation_center = kin_params.wheelbase_length / 2;
    }
    if (!pnode_->getParam("kinematics/minimum_radius_outer", minimum_radius_outer))
    {
      ROS_FATAL_STREAM(pnode_->getNamespace() << "/kinematics/minimum_radius_outer parameter is not set");
      return -1;
    }
    else
    {
      kin_params.minimum_radius = minimum_radius_outer - kin_params.track_width / 2;
    }
    if (!pnode_->getParam("kinematics/steering_ratio", kin_params.steering_ratio))
    {
      ROS_FATAL_STREAM(pnode_->getNamespace() << "/kinematics/steering_ratio parameter is not set");
      return -1;
    }

//    kin_(2.7, 1.626, 1.35);
    kin_ = boost::make_shared<VehicleKinematics>(kin_params);

    sub_ = node_->subscribe<kut_ugv_msgs::MotionCommandStamped>("motion_command", 200,
                                                                &CompactRIOCommunicationNode::motionCommand, this);
    odom_pub_ = node_->advertise<nav_msgs::Odometry>("odom", 200);
    state_pub_ = node_->advertise<kut_ugv_vehicle::StateStamped>("state", 200);

    tf_br_ = boost::shared_ptr<tf::TransformBroadcaster>(new tf::TransformBroadcaster);

    timeout_ = boost::posix_time::seconds(socket_timeout_);
    send_ep_ = udp::endpoint(boost::asio::ip::address::from_string(crio_ip_), std::atoi(crio_cmd_port_.c_str()));
    receive_ep_ = udp::endpoint(udp::v4(), std::atoi(crio_state_port_.c_str()));

    socket_.open(udp::v4());

    deadline_.expires_at(boost::posix_time::pos_infin);
    this->deadlineCallback(deadline_, socket_);

    return 0;
  }
Пример #13
0
void subscribe() {
    subscriber = nodeHandle->subscribe(subscriberTopic, subscriberQueueSize,
                                       &callback, subscriberTransportHints);
}
Пример #14
0
  int init(PhantomState *s)
  {
    if(!s)
    {
      ROS_FATAL("Internal error. PhantomState is NULL.");
      return -1;
    }

    node_ = ros::NodeHandlePtr(new ros::NodeHandle);
    pnode_ = ros::NodeHandlePtr(new ros::NodeHandle("~"));
    pnode_->param(std::string("tf_prefix"), tf_prefix_, std::string(""));

    // Vertical displacement from base_link to link_0. Defaults to Omni offset.
    pnode_->param(std::string("table_offset"), table_offset_, .135);

    // Force feedback damping coefficient
    pnode_->param(std::string("damping_k"), damping_k_, 0.001);

    // On startup device will generate forces to hold end-effector at origin.
    pnode_->param(std::string("locked"), locked_, false);

    // Check calibration status on start up and calibrate if necessary.
    pnode_->param(std::string("calibrate"), calibrate_, false);

    //Frame attached to the base of the phantom (NAME/base_link)
    base_link_name_ = "base_link";

    //Publish on NAME/pose
    std::string pose_topic_name = "pose";
    pose_publisher_ = node_->advertise<geometry_msgs::PoseStamped>(pose_topic_name, 100);

    //Publish button state on NAME/button
    std::string button_topic = "button";
    button_publisher_ = node_->advertise<sensable_phantom::PhantomButtonEvent>(button_topic, 100);

    //Subscribe to NAME/force_feedback
    std::string force_feedback_topic = "force_feedback";
    wrench_sub_ = node_->subscribe(force_feedback_topic, 100, &PhantomROS::wrench_callback, this);

    //Frame of force feedback (NAME/sensable_origin)
    sensable_frame_name_ = "sensable_origin";

    for (int i = 0; i < 7; i++)
    {
      std::ostringstream stream1;
      stream1 << "link_" << i;
      link_names_[i] = std::string(stream1.str());
    }

    state_ = s;
    state_->buttons[0] = 0;
    state_->buttons[1] = 0;
    state_->buttons_prev[0] = 0;
    state_->buttons_prev[1] = 0;
    hduVector3Dd zeros(0, 0, 0);
    state_->velocity = zeros;
    state_->inp_vel1 = zeros; //3x1 history of velocity
    state_->inp_vel2 = zeros; //3x1 history of velocity
    state_->inp_vel3 = zeros; //3x1 history of velocity
    state_->out_vel1 = zeros; //3x1 history of velocity
    state_->out_vel2 = zeros; //3x1 history of velocity
    state_->out_vel3 = zeros; //3x1 history of velocity
    state_->pos_hist1 = zeros; //3x1 history of position
    state_->pos_hist2 = zeros; //3x1 history of position
    state_->lock = locked_;
    state_->lock_pos = zeros;
    state_->hd_cur_transform = hduMatrix::createTranslation(0, 0, 0);

    return 0;
  }
Пример #15
0
	void init()
	{
		/*
		This is the initialize funtion to load configuration parameters from launch file
		*/
		// Ros Node
		node_ = ros::NodeHandlePtr(new ros::NodeHandle("~"));
			std::cout << "Starting with the following parameters:" << std::endl;
		// Settings
		node_->param(std::string("publish_rate"), publish_rate_, PUBLISH_RATE);
			std::cout << "\t" << "publish_rate = " << publish_rate_ << std::endl;
		node_->param(std::string("master1"), master1_, false);
					std::cout << "\t" << "master 1 = " << master1_ << std::endl;
		node_->param(std::string("master2"), master2_, false);
					std::cout << "\t" << "master 2 = " << master2_ << std::endl;
		node_->param(std::string("slave"), slave_, false);
					std::cout << "\t" << "slave = " << slave_ << std::endl;
		node_->param(std::string("popc_enable"), popc_enable_, false);
					std::cout << "\t" << "popc_enable = " << popc_enable_ << std::endl;
		// Time delay
		node_->param(std::string("time_delay"), time_delay_, TIME_DELAY);
			std::cout << "\t" << "time_delay = " << time_delay_ << std::endl;
		// Subscriber settings
		// actual velocity
		node_->param(std::string("actual_velocity_src_name"), actual_velocity_src_name_, std::string(""));
		if (!actual_velocity_src_name_.empty())
		{
			node_->param(std::string("actual_velocity_topic_name"), actual_velocity_topic_name_, std::string("velocity"));
			std::cout << "\t" << "actual_velocity_topic_name = " << actual_velocity_topic_name_ << std::endl;

			actual_velocity_sub_ = node_->subscribe<brl_teleop_msgs::Vel>(actual_velocity_src_name_ + std::string("/") +
					actual_velocity_topic_name_, 100,&controller::actualVelocityCallback,this);
		}
		// Velocity Master - Master
		node_->param(std::string("velocity_master_master_src_name"), velocity_master_master_src_name_, std::string(""));
		if(!velocity_master_master_src_name_.empty())
		{
			node_->param(std::string("velocity_master_master_topic_name"), velocity_master_master_topic_name_, std::string("velocity_master_master"));
			std::cout << "\t" << "velocity_master_master_topic_name = " << velocity_master_master_topic_name_ << std::endl;

			velocity_master_master_sub_ = node_->subscribe<brl_teleop_msgs::Package>(velocity_master_master_src_name_ + std::string("/") + velocity_master_master_topic_name_, 100,
								&controller::velocityMasterToMasterCallback,this);
		}
		// Force Master - Master
		node_->param(std::string("force_master_master_src_name"), force_master_master_src_name_, std::string(""));
		if(!force_master_master_src_name_.empty())
		{
			node_->param(std::string("force_master_master_topic_name"), force_master_master_topic_name_, std::string("force_master_master"));
			std::cout << "\t" << "force_master_master_topic_name = " << force_master_master_topic_name_ << std::endl;

			force_master_master_sub_ = node_->subscribe<brl_teleop_msgs::Package>(force_master_master_src_name_ + std::string("/") + force_master_master_topic_name_, 100,
								&controller::forceMasterToMasterCallback,this);
		}

		// Velocity Master1 - Slave
		node_->param(std::string("velocity_master1_slave_src_name"), velocity_master1_slave_src_name_, std::string(""));
		if(!velocity_master1_slave_src_name_.empty())
		{
			node_->param(std::string("velocity_master1_slave_topic_name"), velocity_master1_slave_topic_name_, std::string("velocity_master_slave"));
			std::cout << "\t" << "velocity_master1_slave_topic_name = " << velocity_master1_slave_topic_name_ << std::endl;

			velocity_master1_slave_sub_ = node_->subscribe<brl_teleop_msgs::Package>(velocity_master1_slave_src_name_ + std::string("/") + velocity_master1_slave_topic_name_, 100,
								&controller::velocityMaster1ToSlaveCallback,this);
		}
		// Velocity Master2 - Slave
		node_->param(std::string("velocity_master2_slave_src_name"), velocity_master2_slave_src_name_, std::string(""));
		if(!velocity_master2_slave_src_name_.empty())
		{
			node_->param(std::string("velocity_master2_slave_topic_name"), velocity_master2_slave_topic_name_, std::string("velocity_master_slave"));
			std::cout << "\t" << "velocity_master2_slave_topic_name = " << velocity_master2_slave_topic_name_ << std::endl;

			velocity_master2_slave_sub_ = node_->subscribe<brl_teleop_msgs::Package>(velocity_master2_slave_src_name_ + std::string("/") + velocity_master2_slave_topic_name_, 100,
								&controller::velocityMaster2ToSlaveCallback,this);
		}

		// Force Slave - Master
		node_->param(std::string("force_slave_master_src_name"), force_slave_master_src_name_, std::string(""));
		if(!force_slave_master_src_name_.empty())
		{
			node_->param(std::string("force_slave_master_topic_name"), force_slave_master_topic_name_, std::string("force_slave_master"));
			std::cout << "\t" << "force_slave_master_topic_name = " << force_slave_master_topic_name_ << std::endl;

			force_slave_master_sub_ = node_->subscribe<brl_teleop_msgs::Package>(force_slave_master_src_name_ + std::string("/") + force_slave_master_topic_name_, 100,
								&controller::forceSlaveToMasterCallback,this);
		}
		
		//PID
		node_->param(std::string("gains/P"), Kp_, 0.1);
		node_->param(std::string("gains/D"), Kd_, 0.01);

		std::cout << "\t" << "Gains = { P: " << Kp_ << ", D: " << Kd_ << "}" << std::endl;

		// Publishing settings
		velocity_master_master_pub_ = node_->advertise<brl_teleop_msgs::Package>("velocity_master_master", 100);
		velocity_master_slave_pub_ = node_->advertise<brl_teleop_msgs::Package>("velocity_master_slave", 100);
		
		force_master_master_pub_ = node_->advertise<brl_teleop_msgs::Package>("force_master_master", 100);
		force_slave_master1_pub_ = node_->advertise<brl_teleop_msgs::Package>("force_slave_master1", 100);
		force_slave_master2_pub_ = node_->advertise<brl_teleop_msgs::Package>("force_slave_master2", 100);
		
		force_popc_pub_ = node_->advertise<brl_teleop_msgs::Package>("force_popc", 100);
	}