Ejemplo n.º 1
0
  virtual void onInit()
  {
    nh_ = getNodeHandle();
    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
    local_nh_ = ros::NodeHandle("~");

    local_nh_.param("debug_view", debug_view_, false);
    subscriber_count_ = 0;
    prev_stamp_ = ros::Time(0, 0);

    window_name_ = "Hough Circle Detection Demo";
    canny_threshold_initial_value_ = 200;
    accumulator_threshold_initial_value_ = 50;
    max_accumulator_threshold_ = 200;
    max_canny_threshold_ = 255;

    //declare and initialize both parameters that are subjects to change
    canny_threshold_ = canny_threshold_initial_value_;
    accumulator_threshold_ = accumulator_threshold_initial_value_;

    image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&HoughCirclesNodelet::img_connectCb, this, _1);
    image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&HoughCirclesNodelet::img_disconnectCb, this, _1);
    ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&HoughCirclesNodelet::msg_connectCb, this, _1);
    ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&HoughCirclesNodelet::msg_disconnectCb, this, _1);
    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
    msg_pub_ = local_nh_.advertise<opencv_apps::CircleArrayStamped>("circles", 1, msg_connect_cb, msg_disconnect_cb);

    if( debug_view_ ) {
      subscriber_count_++;
    }

    dynamic_reconfigure::Server<hough_circles::HoughCirclesConfig>::CallbackType f =
      boost::bind(&HoughCirclesNodelet::reconfigureCallback, this, _1, _2);
    srv.setCallback(f);
  }
Ejemplo n.º 2
0
    virtual void onInit()
    {
        nh_ = getNodeHandle();
        it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
        local_nh_ = ros::NodeHandle("~");

        local_nh_.param("debug_view", debug_view_, false);
        subscriber_count_ = 0;
        prev_stamp_ = ros::Time(0, 0);

        window_name_ = "Hough Lines Demo";
        min_threshold_ = 50;
        max_threshold_ = 150;
        threshold_ = max_threshold_;

        image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&HoughLinesNodelet::img_connectCb, this, _1);
        image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&HoughLinesNodelet::img_disconnectCb, this, _1);
        ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&HoughLinesNodelet::msg_connectCb, this, _1);
        ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&HoughLinesNodelet::msg_disconnectCb, this, _1);
        img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
        msg_pub_ = local_nh_.advertise<opencv_apps::LineArrayStamped>("lines", 1, msg_connect_cb, msg_disconnect_cb);

        if( debug_view_ ) {
            subscriber_count_++;
        }

        dynamic_reconfigure::Server<hough_lines::HoughLinesConfig>::CallbackType f =
            boost::bind(&HoughLinesNodelet::reconfigureCallback, this, _1, _2);
        srv.setCallback(f);
    }
  virtual void onInit()
  {
    nh_ = getNodeHandle();
    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
    local_nh_ = ros::NodeHandle("~");

    local_nh_.param("debug_view", debug_view_, false);
    subscriber_count_ = 0;
    prev_stamp_ = ros::Time(0, 0);

    window_name_ = "Demo code to find contours in an image";
    low_threshold_ = 100; // only for canny

    image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&FindContoursNodelet::img_connectCb, this, _1);
    image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&FindContoursNodelet::img_disconnectCb, this, _1);
    ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&FindContoursNodelet::msg_connectCb, this, _1);
    ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&FindContoursNodelet::msg_disconnectCb, this, _1);
    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
    msg_pub_ = local_nh_.advertise<opencv_apps::ContourArrayStamped>("contours", 1, msg_connect_cb, msg_disconnect_cb);
        
    if( debug_view_ ) {
      subscriber_count_++;
    }

    dynamic_reconfigure::Server<find_contours::FindContoursConfig>::CallbackType f =
      boost::bind(&FindContoursNodelet::reconfigureCallback, this, _1, _2);
    srv.setCallback(f);
  }
        object_segmentation_ros()
        {
       	
  			f = boost::bind(&object_segmentation_ros::configure_callback, this, _1, _2);
  			server.setCallback(f);
        	
        	
        		std::string get_scene_objects_remap;
        		n_.param("get_scene_objects_remap", get_scene_objects_remap, (std::string)"get_scene_objects");
        		get_scene_objects_ = n_.advertiseService<brics_3d_msgs::GetSceneObjects::Request , brics_3d_msgs::GetSceneObjects::Response>(get_scene_objects_remap, boost::bind(&object_segmentation_impl::callback_get_scene_objects, &component_implementation_,_1,_2,component_config_));
        
				object_points_ = 	n_.advertise<sensor_msgs::PointCloud2>("object_points", 1);
				plane_points_ = 	n_.advertise<sensor_msgs::PointCloud2>("plane_points", 1);
  	

				n_.param("camera_frame", component_config_.camera_frame, (std::string)"/openni_rgb_optical_frame");
				n_.param("extract_obj_in_rgb_img", component_config_.extract_obj_in_rgb_img, (bool)false);
				n_.param("min_x", component_config_.min_x, (double)0.25);
				n_.param("max_x", component_config_.max_x, (double)1.5);
				n_.param("min_y", component_config_.min_y, (double)-1.0);
				n_.param("max_y", component_config_.max_y, (double)1.0);
				n_.param("min_z", component_config_.min_z, (double)-0.1);
				n_.param("max_z", component_config_.max_z, (double)1.0);
				n_.param("threshold_points_above_lower_plane", component_config_.threshold_points_above_lower_plane, (double)0.01);
				n_.param("downsampling_distance", component_config_.downsampling_distance, (double)0.005);
				n_.param("min_points_per_objects", component_config_.min_points_per_objects, (int)11);
				n_.param("spherical_distance", component_config_.spherical_distance, (double)2.5);
            
        }
Ejemplo n.º 5
0
 PassthroughCarBody(){
   ROS_INFO("START SUBSCRIBING");
   point_sub = n.subscribe("input_points2", 1, &PassthroughCarBody::passthrough_cb, this);
   point_pub = n.advertise<sensor_msgs::PointCloud2>("passthrough_output/points2", 1);
   f = boost::bind(&PassthroughCarBody::dynamic_reconfigure_cb, this, _1, _2);
   server.setCallback(f);
 }
  virtual void onInit()
  {
    nh_ = getNodeHandle();
    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
    local_nh_ = ros::NodeHandle("~");

    local_nh_.param("debug_view", debug_view_, false);
    subscriber_count_ = 0;
    prev_stamp_ = ros::Time(0, 0);

    image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&FaceDetectionNodelet::img_connectCb, this, _1);
    image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&FaceDetectionNodelet::img_disconnectCb, this, _1);
    ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&FaceDetectionNodelet::msg_connectCb, this, _1);
    ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&FaceDetectionNodelet::msg_disconnectCb, this, _1);
    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
    msg_pub_ = local_nh_.advertise<opencv_apps::FaceArrayStamped>("faces", 1, msg_connect_cb, msg_disconnect_cb);
        
    if( debug_view_ ) {
      subscriber_count_++;
    }
    std::string face_cascade_name, eyes_cascade_name;
    local_nh_.param("face_cascade_name", face_cascade_name, std::string("/usr/share/opencv/haarcascades/haarcascade_frontalface_alt.xml"));
    local_nh_.param("eyes_cascade_name", eyes_cascade_name, std::string("/usr/share/opencv/haarcascades/haarcascade_eye_tree_eyeglasses.xml"));
        
    if( !face_cascade_.load( face_cascade_name ) ){ NODELET_ERROR("--Error loading %s", face_cascade_name.c_str()); };
    if( !eyes_cascade_.load( eyes_cascade_name ) ){ NODELET_ERROR("--Error loading %s", eyes_cascade_name.c_str()); };

    dynamic_reconfigure::Server<face_detection::FaceDetectionConfig>::CallbackType f =
      boost::bind(&FaceDetectionNodelet::reconfigureCallback, this, _1, _2);
    srv.setCallback(f);
  }
Ejemplo n.º 7
0
  virtual void onInit()
  {
    nh_ = getNodeHandle();
    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
    local_nh_ = ros::NodeHandle("~");

    local_nh_.param("debug_view", debug_view_, false);
    subscriber_count_ = 0;
    prev_stamp_ = ros::Time(0, 0);

    window_name_ = "flow";

    image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&FBackFlowNodelet::img_connectCb, this, _1);
    image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&FBackFlowNodelet::img_disconnectCb, this, _1);
    ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&FBackFlowNodelet::msg_connectCb, this, _1);
    ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&FBackFlowNodelet::msg_disconnectCb, this, _1);
    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
    msg_pub_ = local_nh_.advertise<opencv_apps::FlowArrayStamped>("flows", 1, msg_connect_cb, msg_disconnect_cb);

    if( debug_view_ ) {
      subscriber_count_++;
    }

    dynamic_reconfigure::Server<fback_flow::FBackFlowConfig>::CallbackType f =
      boost::bind(&FBackFlowNodelet::reconfigureCallback, this, _1, _2);
    srv.setCallback(f);
  }
Ejemplo n.º 8
0
        AngularRateControl() : nh_("~"),lastIMU_(0.0),
        lastCommand_(0.0), timeout_(1.5),
        joy_control_("/teleop/twistCommand"), auto_control_("/mux/safeCommand"),
        manual_override_(false), rc_override_(true)
         {
            nh_.param("kp",kp_,1.0);
            nh_.param("ki",ki_,0.0);
            nh_.param("kd",kd_,0.0);
            nh_.param("imax",imax_,1E9);
            nh_.param("period",period_,0.05);
            nh_.param("motor_zero",motor_zero_,0.0);
            nh_.param("input_scale",input_scale_,1.0);
            double dTimeout;
            nh_.param("timeout",dTimeout,1.5);
            timeout_ = ros::Duration(dTimeout);

            Pid_.initPid(kp_,ki_,kd_,imax_,-imax_);

            // Make sure TF is ready
            ros::Duration(0.5).sleep();

            // Subscribe to the velocity command and setup the motor publisher
            imu_sub_ = nh_.subscribe("imu",1,&AngularRateControl::imu_callback,this);
            cmd_sub_ = nh_.subscribe("command",1,&AngularRateControl::cmd_callback,this);
            mux_sub_ = nh_.subscribe("/mux/selected",1,&AngularRateControl::mux_callback,this);
            rc_sub_ = nh_.subscribe("/sense",1,&AngularRateControl::rc_callback,this);
            motor_pub_ = nh_.advertise<geometry_msgs::Twist>("motors",1);
            debug_pub_ = nh_.advertise<geometry_msgs::Point>("debug",1);
            //Create a callback for the PID loop
            pid_loop_ = nh_.createTimer(ros::Duration(period_), &AngularRateControl::do_pid, this);

            f_ = boost::bind(&AngularRateControl::config_callback, this, _1, _2);
            server_.setCallback(f_);

        }
    eurobot2016_robotclaw_driver_ros() : np_("~")
    {
        f = boost::bind(&eurobot2016_robotclaw_driver_ros::configure_callback, this, _1, _2);
        server.setCallback(f);


        cmd_vel_ = n_.subscribe("cmd_vel", 1, &eurobot2016_robotclaw_driver_impl::topicCallback_cmd_vel, &component_implementation_);

    }
Ejemplo n.º 10
0
    Omni_Vision(int argc, char **argv)
    {
        char * environment;
        if((environment = getenv("AGENT"))==NULL)
        {
            ROS_ERROR("this agent number is not read by robot");
            return ;
        }
        Agent_ID_ = atoi(environment);
        std::stringstream ss;
        ss<<Agent_ID_;
        std::string calibration_path="/home/nubot"+ss.str()+"/nubot_ws/src/nubot/omni_vision/calib_results";
        if(argc>1)
            calibration_path=argv[1];

        ROS_INFO("initialize the omni_vision  process");
        imginfo_     = new OmniImage(calibration_path+"/"+ss.str()+"/ROI.xml");
        tranfer_     = new Transfer(calibration_path+"/"+ss.str()+"/mirror_calib.xml",*imginfo_);
        scanpts_     = new ScanPoints(*imginfo_);
        whites_      = new Whites(*scanpts_,*tranfer_);
        optimise_    = new Optimise(calibration_path+"/errortable.bin",
                                    calibration_path+"/Diff_X.bin",
                                    calibration_path+"/Diff_Y.bin",
                                    *tranfer_);
        glocation_   = new Globallocalization(*optimise_);
        odometry_    = new Odometry();
        location_    = new Localization(*optimise_);
        obstacles_   = new Obstacles(*scanpts_,*tranfer_);
        ball_finder_ = new BallFinder(*tranfer_);
        colorsegment_= new ColorSegment(calibration_path+"/"+ss.str()+"/CTable.dat");

        is_show_ball_=false;
        is_show_obstacles_=false;
        is_show_whites_=false;
        is_show_scan_points=false;
        is_show_result_=false;
        is_robot_stuck_ = false;

        robot.isglobal_=true;
        is_restart_=false;
        is_power_off_=false;
        if(WIDTH_RATIO<1)
            field_image_ = cv::imread(calibration_path+"/"+ss.str()+"/field_mine.bmp");
        else
            field_image_ = cv::imread(calibration_path+"/"+ss.str()+"/field.bmp");

        ros::NodeHandle nh;
        image_transport::ImageTransport it(nh);
        img_sub_= it.subscribe("/camera/image_raw", 1, &Omni_Vision::imageCallback,this);
        ros::NodeHandle local_nh;
        motor_info_sub_ = local_nh.subscribe("/nubotdriver/odoinfo", 1, &Omni_Vision::odometryupdate,this);

        ros::NodeHandle node;
        omin_vision_pub_   = node.advertise<nubot_common::OminiVisionInfo>("/omnivision/OmniVisionInfo",1);
        reconfigureServer_.setCallback(boost::bind(&Omni_Vision::configure, this, _1, _2));
    }
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));
    
}
    common_servo_converter_ros() : np_("~")
    {
        f = boost::bind(&common_servo_converter_ros::configure_callback, this, _1, _2);
        server.setCallback(f);


        output_ = n_.advertise<std_msgs::UInt16>("output", 1);
        input_ = n_.subscribe("input", 1, &common_servo_converter_impl::topicCallback_input, &component_implementation_);

        np_.param("inverted", component_config_.inverted, (int)0);
        np_.param("offset", component_config_.offset, (int)0);
    }
    RectangleDetector(ros::NodeHandle nh = ros::NodeHandle()) : it_(nh), nh_(nh)
    {

        image_sub = new message_filters::Subscriber<sensor_msgs::Image>(nh_, "image", 1);
        line_sub = new message_filters::Subscriber<jsk_perception::LineArray>(nh_, "lines", 1);
        sync = new TimeSynchronizer<sensor_msgs::Image, jsk_perception::LineArray>(*image_sub, *line_sub, 10);
        sync->registerCallback(boost::bind(&RectangleDetector::callback, this, _1, _2));
        image_pub_ = nh_.advertise<sensor_msgs::Image>("rectangle/image", 1);

        dynamic_reconfigure::Server<jsk_perception::RectangleDetectorConfig>::CallbackType f =
            boost::bind(&RectangleDetector::reconfigureCallback, this, _1, _2);
        srv.setCallback(f);
    }
Ejemplo n.º 14
0
    HoughLines(ros::NodeHandle nh = ros::NodeHandle()) : it_(nh), nh_(nh), subscriber_count_(0)
    {
        image_transport::SubscriberStatusCallback connect_cb    = boost::bind(&HoughLines::connectCb, this, _1);
        image_transport::SubscriberStatusCallback disconnect_cb = boost::bind(&HoughLines::disconnectCb, this, _1);
        img_pub_ = image_transport::ImageTransport(ros::NodeHandle(nh_, "hough")).advertise("image", 1, connect_cb, disconnect_cb);

        out_pub_ = nh.advertise<jsk_perception::LineArray>(nh_.resolveName("lines"), 1);


        dynamic_reconfigure::Server<jsk_perception::HoughLinesConfig>::CallbackType f =
            boost::bind(&HoughLines::reconfigureCallback, this, _1, _2);
        srv.setCallback(f);
    }
Ejemplo n.º 15
0
  /** get initial parameters (only when node starts). */
  void getInitParams(void)
  {
    nh_.param("modulation_freq", modulation_freq_, -1);
    std::string default_addr("");
    nh_.param("ether_addr",  ether_addr_, default_addr);
    cinfo_ = new camera_info_manager::CameraInfoManager(nh_);

    nh_.param("camera_name",  camera_name_, std::string("swissranger"));

    nh_.param("use_filter", use_filter_, static_cast<bool>(USE_FILTER));
    dynamic_reconfigure::Server<swissranger_camera::SwissRangerConfig >::CallbackType f
      = boost::bind(&SRNode::reconfig, this, _1, _2);
    dynsrv.setCallback(f);
  }
Ejemplo n.º 16
0
        kr16_node_ros()
        :as_follow_joint_trajectory_action_(n_, "follow_joint_trajectory_action", boost::bind(&kr16_node_impl::callback_follow_joint_trajectory_action_, &component_implementation_, _1, &as_follow_joint_trajectory_action_), false)
        {
       	
  			f = boost::bind(&kr16_node_ros::configure_callback, this, _1, _2);
  			server.setCallback(f);
 			as_follow_joint_trajectory_action_.start();
        	
        
				joint_states_ = 	n_.advertise<sensor_msgs::JointState>("joint_states", 1);
				state_ = 	n_.advertise<control_msgs::JointTrajectoryControllerState>("state", 1);
  	

				n_.param("robot_ip_address", component_config_.robot_ip_address, (std::string)"192.1.10.20");
				n_.param("robot_description", component_config_.robot_description, (std::string)"/home/ros/");
				n_.param("robot_port", component_config_.robot_port, (int)49152);
            
        }
  ColorHistogramDescriptorNode() {
    ros::NodeHandle nh;

    dynamic_reconfigure::Server<saliency_detector::color_histogram_descriptor_paramsConfig>::CallbackType cb;
    cb = boost::bind(&ColorHistogramDescriptorNode::configCallback, this,  _1, _2);
    dr_srv.setCallback(cb);

    ros::NodeHandle private_node_handle_("~");

    sub_patch_array =
      nh.subscribe("projected_patch_array", 1, &ColorHistogramDescriptorNode::patchArrayCallback, this);

    pub_named_points =
      nh.advertise<samplereturn_msgs::NamedPointArray>("named_point", 1);

    pub_debug_image =
      nh.advertise<sensor_msgs::Image>("color_debug_image", 1);

  }
Ejemplo n.º 18
0
	ArdroneController(ros::NodeHandle& nh) :
			nh(nh), reconfigure_server(), enabled(false) {
		pub_vel = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
		pub_cmd_marker = nh.advertise<visualization_msgs::Marker>(
				"visualization_marker", 10);

		reconfigure_server.setCallback(
				boost::bind(&ArdroneController::onConfig, this, _1, _2));

		twist.linear.x = twist.linear.y = twist.linear.z = 0;
		twist.angular.x = twist.angular.y = twist.angular.z = 0;
		setGoalPose(0, 0, 0);

		sub_enabled = nh.subscribe<std_msgs::Bool>("/ardrone/enable_controller",
				1,
				boost::bind(&ArdroneController::onEnableController, this, _1));
		sub_pose = nh.subscribe<visnav2013_exercise3::State>(
				"/ardrone/filtered_pose", 1,
				boost::bind(&ArdroneController::onFilteredPose, this, _1));
	}
Ejemplo n.º 19
0
  /*!
  * \brief Default contructor.
  *
  * This description is displayed lower in the doxygen as an extended description along with
  * the above brief description.
  */
  HapticNode(char* argv[]):
      nh_("/")
      , nh_pvt_("~")
      , input_topic_("/cloud_in")
      , output_topic_("/cloud_out")
      //, dyn_srv(ros::NodeHandle("~config")),
  {
    got_first_cloud_ = false;
    now_ = ros::Time::now() - ros::Duration(1.0);

    // Clouds in and out
    sub_cloud_ = nh_.subscribe<sensor_msgs::PointCloud2>
                 (input_topic_, 1, boost::bind(&HapticNode::cloudCallback,this, _1));
    pub_cloud_ = nh_.advertise<sensor_msgs::PointCloud2>(output_topic_, 1);
    pub_marker_cloud_ = nh_.advertise<visualization_msgs::MarkerArray>("/marker_cloud_array", 1);

    // Disparity images in and out:
    sub_disparity_1_ = nh_.subscribe<stereo_msgs::DisparityImage>
                 ("/disparity_1", 1, boost::bind(&HapticNode::disparityCallback, this, _1));

    // Marker topics
    pub_marker_ = nh_.advertise<visualization_msgs::Marker>("/markers", 100);
    pub_marker_array_ = nh_.advertise<visualization_msgs::MarkerArray>("/markers_array", 1);

    // String status topic
    pub_status_ = nh_pvt_.advertise<std_msgs::String>("status", 10);

    // Create timer callback for auto-running of algorithm
    update_timer_ = nh_.createTimer(ros::Duration(0.03333), boost::bind(&HapticNode::timerCallback, this));
    slow_update_timer_ = nh_.createTimer(ros::Duration(0.5), boost::bind(&HapticNode::slowTimerCallback, this));


    // This server is for the user to adjust the algorithm weights
    dyn_cb = boost::bind( &HapticNode::dynamicCallback, this, _1, _2 );
    dyn_srv.setCallback(dyn_cb);

    // Set up the chai world and device
    initializeHaptics();

    ROS_INFO("Finished constructor!");
  }
Ejemplo n.º 20
0
    ForceField(ros::NodeHandle & n_, double & freq_, double & ro_,   Eigen::Vector3d kp_, Eigen::Vector3d kd_, double & laser_max_distance_, double & robot_mass_, double & robot_radius_, std::string & pose_topic_name_, std::string & sonar_topic_name_) :
    n(n_), freq(freq_), ro(ro_), kp(kp_), kd(kd_), laser_max_distance(laser_max_distance_), robot_mass(robot_mass_), robot_radius(robot_radius_), pose_topic_name(pose_topic_name_), sonar_topic_name(sonar_topic_name_)
    {
        std::cout << "In the class" << std::endl ;
        kp_mat << kp.x(), 0, 0,
                0, kp.y(), 0,
                0, 0, kp.z();
        kd_mat << kd.x(), 0, 0,
                0, kd.y(), 0,
                0, 0, kd.z();


        param_callback_type = boost::bind(&ForceField::paramsCallback, this, _1, _2);
        param_server.setCallback(param_callback_type);
        visualization_markers_pub = n.advertise<visualization_msgs::MarkerArray>( "/force_field_markers", 1);
       // repulsive_force_out = n.advertise<geometry_msgs::Twist>( "/feedback_force/repulsive_force", 1);
        force_out = n.advertise<phantom_omni::OmniFeedback>( "/omni1_force_feedback", 1);
        init_flag=false;
        obstacle_readings_sub = n.subscribe("/cloud", 100, &ForceField::sonarCallback, this);
        lastTimeCalled = ros::Time::now().toSec();
};
    beacon_robot_pose_estimate_ros() : np_("~")
    {
        f = boost::bind(&beacon_robot_pose_estimate_ros::configure_callback, this, _1, _2);
        server.setCallback(f);


        robot1_pose_ = n_.advertise<geometry_msgs::Pose2D>("robot1_pose", 1);
        robot2_pose_ = n_.advertise<geometry_msgs::Pose2D>("robot2_pose", 1);
        input_cloud_ = n_.subscribe("input_cloud", 1, &beacon_robot_pose_estimate_impl::topicCallback_input_cloud, &component_implementation_);
        odom1_ = n_.subscribe("odom1", 1, &beacon_robot_pose_estimate_impl::topicCallback_odom1, &component_implementation_);
        odom2_ = n_.subscribe("odom2", 1, &beacon_robot_pose_estimate_impl::topicCallback_odom2, &component_implementation_);

        np_.param("x_robot1", component_config_.x_robot1, (double)0.0);
        np_.param("y_robot1", component_config_.y_robot1, (double)0.0);
        np_.param("x_robot2", component_config_.x_robot2, (double)0.0);
        np_.param("y_robot2", component_config_.y_robot2, (double)0.0);
        np_.param("world_link", component_config_.world_link, (std::string)"world");
        np_.param("robot1_link", component_config_.robot1_link, (std::string)"robot1_base_link");
        np_.param("robot2_link", component_config_.robot2_link, (std::string)"robot2_base_link");
        np_.param("detection_distance", component_config_.detection_distance, (double)0.07);
    }
        schunk_gripper_ros()
        {
       	
  			f = boost::bind(&schunk_gripper_ros::configure_callback, this, _1, _2);
  			server.setCallback(f);
        	
        	
        		std::string MoveGripper_remap;
        		n_.param("MoveGripper_remap", MoveGripper_remap, (std::string)"MoveGripper");
        		MoveGripper_ = n_.advertiseService<brics_showcase_industry_interfaces::MoveGripper::Request , brics_showcase_industry_interfaces::MoveGripper::Response>(MoveGripper_remap, boost::bind(&schunk_gripper_impl::callback_MoveGripper, &component_implementation_,_1,_2,component_config_));
        
  	

				n_.param("dev_string", component_config_.dev_string, (std::string)"/dev/pcan32");
				n_.param("open_pos", component_config_.open_pos, (double)1.0);
				n_.param("close_pos", component_config_.close_pos, (double)1.0);
				n_.param("baudrate", component_config_.baudrate, (int)1000);
				n_.param("modul_id", component_config_.modul_id, (int)12);
				n_.param("speed", component_config_.speed, (double)0.01);
            
        }
Ejemplo n.º 23
0
  virtual void onInit()
  {
    Nodelet::onInit();
    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(*nh_));

    pnh_->param("debug_view", debug_view_, false);
    if (debug_view_) {
      always_subscribe_ = true;
    }
    prev_stamp_ = ros::Time(0, 0);

    window_name_ = "Hull Demo";
    threshold_ = 100;
    
    dynamic_reconfigure::Server<convex_hull::ConvexHullConfig>::CallbackType f =
      boost::bind(&ConvexHullNodelet::reconfigureCallback, this, _1, _2);
    srv.setCallback(f);

    img_pub_ = advertiseImage(*pnh_, "image", 1);
    msg_pub_ = advertise<opencv_apps::ContourArrayStamped>(*pnh_, "hulls", 1);
    onInitPostProcess();
  }
    home_command_manager_ros() : np_("~")
    {
        f = boost::bind(&home_command_manager_ros::configure_callback, this, _1, _2);
        server.setCallback(f);


        command1_ = n_.advertise<std_msgs::Empty>("command1", 1);
        command2_ = n_.advertise<std_msgs::Empty>("command2", 1);
        command3_ = n_.advertise<std_msgs::Empty>("command3", 1);
        command4_ = n_.advertise<std_msgs::Empty>("command4", 1);
        command5_ = n_.advertise<std_msgs::Empty>("command5", 1);
        command6_ = n_.advertise<std_msgs::Empty>("command6", 1);
        command7_ = n_.advertise<std_msgs::Empty>("command7", 1);
        command8_ = n_.advertise<std_msgs::Empty>("command8", 1);
        command9_ = n_.advertise<std_msgs::Empty>("command9", 1);
        command10_ = n_.advertise<std_msgs::Empty>("command10", 1);
        command11_ = n_.advertise<std_msgs::Empty>("command11", 1);
        command12_ = n_.advertise<std_msgs::Empty>("command12", 1);
        command13_ = n_.advertise<std_msgs::Empty>("command13", 1);
        command14_ = n_.advertise<std_msgs::Empty>("command14", 1);
        command_ = n_.subscribe("command", 1, &home_command_manager_impl::topicCallback_command, &component_implementation_);

    }
Ejemplo n.º 25
0
  virtual void onInit()
  {
    Nodelet::onInit();
    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(*nh_));

    pnh_->param("debug_view", debug_view_, false);
    if (debug_view_) {
      always_subscribe_ = true;
    }
    prev_stamp_ = ros::Time(0, 0);

    window_name_ = "Contours";
    threshold_ = 100;

    dynamic_reconfigure::Server<general_contours::GeneralContoursConfig>::CallbackType f =
      boost::bind(&GeneralContoursNodelet::reconfigureCallback, this, _1, _2);
    srv.setCallback(f);
    
    img_pub_ = advertiseImage(*pnh_, "image", 1);
    rects_pub_ = advertise<opencv_apps::RotatedRectArrayStamped>(*pnh_, "rectangles", 1);
    ellipses_pub_ = advertise<opencv_apps::RotatedRectArrayStamped>(*pnh_, "ellipses", 1);

    onInitPostProcess();
  }
Ejemplo n.º 26
0
int RosAriaNode::Setup()
{
  // Note, various objects are allocated here which are never deleted (freed), since Setup() is only supposed to be
  // called once per instance, and these objects need to persist until the process terminates.

  robot = new ArRobot();
  ArArgumentBuilder *args = new ArArgumentBuilder(); //  never freed
  ArArgumentParser *argparser = new ArArgumentParser(args); // Warning never freed
  argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args.  Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)

  // Now add any parameters given via ros params (see RosAriaNode constructor):

  // if serial port parameter contains a ':' character, then interpret it as hostname:tcpport
  // for wireless serial connection. Otherwise, interpret it as a serial port name.
  size_t colon_pos = serial_port.find(":");
  if (colon_pos != std::string::npos)
  {
    args->add("-remoteHost"); // pass robot's hostname/IP address to Aria
    args->add(serial_port.substr(0, colon_pos).c_str());
    //ROS_INFO( "RosAria: using IP: [%s]", serial_port.substr(0, colon_pos).c_str() );
    args->add("-remoteRobotTcpPort"); // pass robot's TCP port to Aria
    args->add(serial_port.substr(colon_pos+1).c_str());
    //ROS_INFO( "RosAria: using port: [%s]", serial_port.substr(colon_pos+1).c_str() );
  }
  else
  {
    args->add("-robotPort"); // pass robot's serial port to Aria
    args->add(serial_port.c_str());
  }

  // if a baud rate was specified in baud parameter
  if(serial_baud != 0)
  {
    args->add("-robotBaud");
    char tmp[100];
    snprintf(tmp, 100, "%d", serial_baud);
    args->add(tmp);
  }
  
  if( debug_aria )
  {
    // turn on all ARIA debugging
    args->add("-robotLogPacketsReceived"); // log received packets
    args->add("-robotLogPacketsSent"); // log sent packets
    args->add("-robotLogVelocitiesReceived"); // log received velocities
    args->add("-robotLogMovementSent");
    args->add("-robotLogMovementReceived");
    ArLog::init(ArLog::File, ArLog::Verbose, aria_log_filename.c_str(), true);
  }


  // Connect to the robot
  conn = new ArRobotConnector(argparser, robot); // warning never freed
  if (!conn->connectRobot()) {
    ROS_ERROR("RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device.)");
    return 1;
  }

  // causes ARIA to load various robot-specific hardware parameters from the robot parameter file in /usr/local/Aria/params
  if(!Aria::parseArgs())
  {
    ROS_ERROR("RosAria: ARIA error parsing ARIA startup parameters!");
    return 1;
  }

  readParameters();

  // Start dynamic_reconfigure server
  dynamic_reconfigure_server = new dynamic_reconfigure::Server<rosaria::RosAriaConfig>;
  

  /*
   * 横向速度和纵向速度单位为米
*/

  /*
   * 初始化参数的最小值 其中TickMM=10 DriftFactor=-200 Revount=-32760
   */

  // Setup Parameter Minimums
  rosaria::RosAriaConfig dynConf_min;
  dynConf_min.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000;
  dynConf_min.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000;
  // TODO: Fix rqt dynamic_reconfigure gui to handle empty intervals
  // Until then, set unit length interval.
  dynConf_min.lat_accel = ((robot->getAbsoluteMaxLatAccel() > 0.0) ? robot->getAbsoluteMaxLatAccel() : 0.1) / 1000;
  dynConf_min.lat_decel = ((robot->getAbsoluteMaxLatDecel() > 0.0) ? robot->getAbsoluteMaxLatDecel() : 0.1) / 1000;
  dynConf_min.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180;
  dynConf_min.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180;
  
  // I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
  dynConf_min.TicksMM     = 10;
  dynConf_min.DriftFactor = -200;
  dynConf_min.RevCount    = -32760;
  
  dynamic_reconfigure_server->setConfigMin(dynConf_min);
  
  
  // 初始化参数的最大值 其中TickMM=200 DriftFactor=200 Revount=32760
  rosaria::RosAriaConfig dynConf_max;
  dynConf_max.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000;
  dynConf_max.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000;
  // TODO: Fix rqt dynamic_reconfigure gui to handle empty intervals
  // Until then, set unit length interval.
  dynConf_max.lat_accel = ((robot->getAbsoluteMaxLatAccel() > 0.0) ? robot->getAbsoluteMaxLatAccel() : 0.1) / 1000;
  dynConf_max.lat_decel = ((robot->getAbsoluteMaxLatDecel() > 0.0) ? robot->getAbsoluteMaxLatDecel() : 0.1) / 1000;
  dynConf_max.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180;
  dynConf_max.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180;
  
  // I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
  dynConf_max.TicksMM     = 200;
  dynConf_max.DriftFactor = 200;
  dynConf_max.RevCount    = 32760;
  
  dynamic_reconfigure_server->setConfigMax(dynConf_max);
  
   // 初始化参数的默认值
  rosaria::RosAriaConfig dynConf_default;
  dynConf_default.trans_accel = robot->getTransAccel() / 1000;
  dynConf_default.trans_decel = robot->getTransDecel() / 1000;
  dynConf_default.lat_accel   = robot->getLatAccel() / 1000;
  dynConf_default.lat_decel   = robot->getLatDecel() / 1000;
  dynConf_default.rot_accel   = robot->getRotAccel() * M_PI/180;
  dynConf_default.rot_decel   = robot->getRotDecel() * M_PI/180;

  dynConf_default.TicksMM     = TicksMM;
  dynConf_default.DriftFactor = DriftFactor;
  dynConf_default.RevCount    = RevCount;
  
  dynamic_reconfigure_server->setConfigDefault(dynConf_max);
  
  dynamic_reconfigure_server->setCallback(boost::bind(&RosAriaNode::dynamic_reconfigureCB, this, _1, _2));

  // Enable the motors
  robot->enableMotors();

  // disable sonars on startup
  robot->disableSonar();

  // callback will  be called by ArRobot background processing thread for every SIP data packet received from robot
  robot->addSensorInterpTask("ROSPublishingTask", 100, &myPublishCB);

  // Initialize bumpers with robot number of bumpers
  bumpers.front_bumpers.resize(robot->getNumFrontBumpers());
  bumpers.rear_bumpers.resize(robot->getNumRearBumpers());

  // Run ArRobot background processing thread
  robot->runAsync(true);

  return 0;
}
Ejemplo n.º 27
0
 FakeCamParameters() {
   server_.setCallback(boost::bind(&FakeCamParameters::reconfigure_cb, this, _1, _2));
 }
Ejemplo n.º 28
0
int RosAriaNode::Setup()
{
  // Note, various objects are allocated here which are never deleted (freed), since Setup() is only supposed to be
  // called once per instance, and these objects need to persist until the process terminates.

  robot = new ArRobot();

  ArArgumentBuilder *args = new ArArgumentBuilder(); //  never freed
  ArArgumentParser *argparser = new ArArgumentParser(args); // Warning never freed
  argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args.  Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)

  // Now add any parameters given via ros params (see RosAriaNode constructor):

  // if serial port parameter contains a ':' character, then interpret it as hostname:tcpport
  // for wireless serial connection. Otherwise, interpret it as a serial port name.
  size_t colon_pos = serial_port.find(":");
  if (colon_pos != std::string::npos)
  {
    args->add("-remoteHost"); // pass robot's hostname/IP address to Aria
    args->add(serial_port.substr(0, colon_pos).c_str());
    args->add("-remoteRobotTcpPort"); // pass robot's TCP port to Aria
    args->add(serial_port.substr(colon_pos+1).c_str());
  }
  else
  {
    args->add("-robotPort"); // pass robot's serial port to Aria
    args->add(serial_port.c_str());
  }

  // if a baud rate was specified in baud parameter
  if(serial_baud != 0)
  {
    args->add("-robotBaud");
    char tmp[100];
    snprintf(tmp, 100, "%d", serial_baud);
    args->add(tmp);
  }
  
  if( debug_aria )
  {
    // turn on all ARIA debugging
    args->add("-robotLogPacketsReceived"); // log received packets
    args->add("-robotLogPacketsSent"); // log sent packets
    args->add("-robotLogVelocitiesReceived"); // log received velocities
    args->add("-robotLogMovementSent");
    args->add("-robotLogMovementReceived");
    ArLog::init(ArLog::File, ArLog::Verbose, aria_log_filename.c_str(), true);
  }

  // Connect to the robot
  conn = new ArRobotConnector(argparser, robot); // warning never freed
  if (!conn->connectRobot()) {
    ROS_ERROR("RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device.)");
    return 1;
  }

  // causes ARIA to load various robot-specific hardware parameters from the robot parameter file in /usr/local/Aria/params
  if(!Aria::parseArgs())
  {
    ROS_ERROR("RosAria: ARIA error parsing ARIA startup parameters!");
    return 1;
  }

  // Start dynamic_reconfigure server
  dynamic_reconfigure_server = new dynamic_reconfigure::Server<rosaria::RosAriaConfig>;

  robot->lock();

  // Setup Parameter Minimums
  rosaria::RosAriaConfig dynConf_min;

  //arbitrary non-zero values so dynamic reconfigure isn't STUPID
  dynConf_min.trans_vel_max = 0.1; 
  dynConf_min.rot_vel_max = 0.1; 
  dynConf_min.trans_accel = 0.1;
  dynConf_min.trans_decel = 0.1;
  dynConf_min.rot_accel = 0.1;
  dynConf_min.rot_decel = 0.1; 
  
  // I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
  dynConf_min.TicksMM     = 10;
  dynConf_min.DriftFactor = -200;
  dynConf_min.RevCount    = -32760;
  
  dynamic_reconfigure_server->setConfigMin(dynConf_min);
  
  rosaria::RosAriaConfig dynConf_max;
  dynConf_max.trans_vel_max = robot->getAbsoluteMaxTransVel() / 1000.0; 
  dynConf_max.rot_vel_max = robot->getAbsoluteMaxRotVel() *M_PI/180.0; 
  dynConf_max.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000.0;
  dynConf_max.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000.0;
  dynConf_max.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180.0;
  dynConf_max.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180.0;
  
  // I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
  dynConf_max.TicksMM     = 200;
  dynConf_max.DriftFactor = 200;
  dynConf_max.RevCount    = 32760;
  
  dynamic_reconfigure_server->setConfigMax(dynConf_max);


  dynConf_default.trans_vel_max = robot->getTransVelMax() / 1000.0; 
  dynConf_default.rot_vel_max = robot->getRotVelMax() *M_PI/180.0; 
  dynConf_default.trans_accel = robot->getTransAccel() / 1000.0;
  dynConf_default.trans_decel = robot->getTransDecel() / 1000.0;
  dynConf_default.rot_accel   = robot->getRotAccel() * M_PI/180.0;
  dynConf_default.rot_decel   = robot->getRotDecel() * M_PI/180.0;

/*  ROS_ERROR("ON ROBOT NOW\n\
Trans vel max: %f\n\
Rot vel max: %f\n\
\n\
trans accel: %f\n\
trans decel: %f\n\
rot accel: %f\n\
rot decel: %f", robot->getTransVelMax(), robot->getRotVelMax(), robot->getTransAccel(), robot->getTransDecel(), robot->getRotAccel(), robot->getRotDecel());

  ROS_ERROR("IN DEFAULT CONFIG\n\
Trans vel max: %f\n\
Rot vel max: %f\n\
\n\
trans accel: %f\n\
trans decel: %f\n\
rot accel: %f\n\
rot decel: %f\n", dynConf_default.trans_vel_max,  dynConf_default.rot_vel_max, dynConf_default.trans_accel, dynConf_default.trans_decel, dynConf_default.rot_accel, dynConf_default.rot_decel);*/

  TicksMM = robot->getOrigRobotConfig()->getTicksMM();
  DriftFactor = robot->getOrigRobotConfig()->getDriftFactor();
  RevCount = robot->getOrigRobotConfig()->getRevCount();

  dynConf_default.TicksMM     = TicksMM;
  dynConf_default.DriftFactor = DriftFactor;
  dynConf_default.RevCount    = RevCount;
  
  dynamic_reconfigure_server->setConfigDefault(dynConf_default);

  for(int i = 0; i < 16; i++)
  {
    sonar_tf_array[i].header.frame_id = frame_id_base_link;
    std::stringstream _frame_id;
    _frame_id << "sonar" << i;
    sonar_tf_array[i].child_frame_id = _frame_id.str();
    ArSensorReading* _reading = NULL;
    _reading = robot->getSonarReading(i);
    sonar_tf_array[i].transform.translation.x = _reading->getSensorX() / 1000.0;
    sonar_tf_array[i].transform.translation.y = _reading->getSensorY() / 1000.0;
    sonar_tf_array[i].transform.translation.z = 0.19;
    sonar_tf_array[i].transform.rotation = tf::createQuaternionMsgFromYaw(_reading->getSensorTh() * M_PI / 180.0);
  }

  for (int i=0;i<16;i++) {
      sensor_msgs::Range r;
      ranges.data.push_back(r);
  }

  int i=0,j=0;
  if (sonars__crossed_the_streams) {
    i=8;
    j=8;
  }
  for(; i<16; i++) {
    //populate the RangeArray msg
    std::stringstream _frame_id;
    _frame_id << "sonar" << i;
    ranges.data[i].header.frame_id = _frame_id.str();
    ranges.data[i].radiation_type = 0;
    ranges.data[i].field_of_view = 0.2618f; 
    ranges.data[i].min_range = 0.03f;
    ranges.data[i].max_range = 5.0f;
  }

  // Enable the motors
  robot->enableMotors();

  robot->disableSonar();

  // Initialize bumpers with robot number of bumpers
  bumpers.front_bumpers.resize(robot->getNumFrontBumpers());
  bumpers.rear_bumpers.resize(robot->getNumRearBumpers());

  robot->unlock();

  pose_pub = n.advertise<nav_msgs::Odometry>("pose",1000);
  bumpers_pub = n.advertise<rosaria::BumperState>("bumper_state",1000);

  voltage_pub = n.advertise<std_msgs::Float64>("battery_voltage", 1000);
  
  combined_range_pub = n.advertise<rosaria::RangeArray>("ranges", 1000,
    boost::bind(&RosAriaNode::sonarConnectCb,this),
    boost::bind(&RosAriaNode::sonarDisconnectCb, this));

  for(int i =0; i < 16; i++) {
    std::stringstream topic_name;
    topic_name << "range" << i;
    range_pub[i] = n.advertise<sensor_msgs::Range>(topic_name.str().c_str(), 1000,
      boost::bind(&RosAriaNode::sonarConnectCb,this),
      boost::bind(&RosAriaNode::sonarDisconnectCb, this));
  }
  recharge_state_pub = n.advertise<std_msgs::Int8>("battery_recharge_state", 5, true /*latch*/ );
  recharge_state.data = -2;
  state_of_charge_pub = n.advertise<std_msgs::Float32>("battery_state_of_charge", 100);

  motors_state_pub = n.advertise<std_msgs::Bool>("motors_state", 5, true /*latch*/ );
  motors_state.data = false;
  published_motors_state = false;
 
  // subscribe to services
  cmdvel_sub = n.subscribe( "cmd_vel", 1, (boost::function <void(const geometry_msgs::TwistConstPtr&)>)
    boost::bind(&RosAriaNode::cmdvel_cb, this, _1 ));

  // advertise enable/disable services
  enable_srv = n.advertiseService("enable_motors", &RosAriaNode::enable_motors_cb, this);
  disable_srv = n.advertiseService("disable_motors", &RosAriaNode::disable_motors_cb, this);
 
  veltime = ros::Time::now();
  sonar_tf_timer = n.createTimer(ros::Duration(0.033), &RosAriaNode::sonarCallback, this);
  sonar_tf_timer.stop();

  dynamic_reconfigure_server->setCallback(boost::bind(&RosAriaNode::dynamic_reconfigureCB, this, _1, _2));

  // callback will  be called by ArRobot background processing thread for every SIP data packet received from robot
  robot->addSensorInterpTask("ROSPublishingTask", 100, &myPublishCB);

  // Run ArRobot background processing thread
  robot->runAsync(true);

  return 0;
}