예제 #1
0
 explicit BTAction(std::string name) :
     as_(nh_, name, boost::bind(&BTAction::execute_callback, this, _1), false),
     action_name_(name)
 {
     // Starts the action server
     as_.start();
 }
    OrientToLaserReadingAction(ros::NodeHandle nh, std::string name, std::string cmd_vel_topic, std::string linreg_service_name) :
        as_(nh,	name, boost::bind(&OrientToLaserReadingAction::executeActionCB, this, _1), false)
    {
        //as_.registerGoalCallback();
        this->action_name_ = name;
        this->service_name = linreg_service_name;
        this->cmd_vel_topic = cmd_vel_topic;

        nh_ = nh;

        target_distance = TARGET_DISTANCE;
        max_velocity = MAX_VELOCITY;


        ROS_INFO("Register publisher");

        cmd_pub = nh_.advertise<geometry_msgs::Twist>(cmd_vel_topic, 1000); // 1000-element buffer for publisher

        ROS_INFO("Create service clinet");

        client = nh_.serviceClient<BaseScanLinearRegression>(service_name);

        as_.start();

    }
  BlockLogicServer(const std::string name) :
    nh_("~"), 
    interactive_m_server_("block_controls"), 
    action_server_(name, false), 
    action_name_(name), 
    initialized_(false), 
    block_size(0.4)
  {
    // Load parameters from the server.
    nh_.param<double>("bump_size", bump_size, 0.0); // original 0.005

    // ---------------------------------------------------------------------------------------------
    // Read in all seen blocks
    block_sub_ = nh_.subscribe("/cube_block_poses", 1, &BlockLogicServer::addBlocks, this);

    // ---------------------------------------------------------------------------------------------
    // Publish location of blocks??
    pick_place_pub_ = nh_.advertise< geometry_msgs::PoseArray >("/pick_place", 1, true);

    // ---------------------------------------------------------------------------------------------
    // Rviz Visualizations
    marker_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker", 1);

    // ---------------------------------------------------------------------------------------------
    // Register the goal and feeback callbacks for action server
    action_server_.registerGoalCallback(boost::bind(&BlockLogicServer::goalCB, this));
    action_server_.registerPreemptCallback(boost::bind(&BlockLogicServer::preemptCB, this));
    action_server_.start();
  }
예제 #4
0
파일: node.cpp 프로젝트: ErolB/Sub8
  Node()
      : private_nh("~"),
        kill_listener(boost::bind(&Node::killed_callback, this)),
        actionserver(nh, "moveto", false),
        disabled(false) {
    fixed_frame = uf_common::getParam<std::string>(private_nh, "fixed_frame");
    body_frame = uf_common::getParam<std::string>(private_nh, "body_frame");

    limits.vmin_b = uf_common::getParam<subjugator::Vector6d>(private_nh, "vmin_b");
    limits.vmax_b = uf_common::getParam<subjugator::Vector6d>(private_nh, "vmax_b");
    limits.amin_b = uf_common::getParam<subjugator::Vector6d>(private_nh, "amin_b");
    limits.amax_b = uf_common::getParam<subjugator::Vector6d>(private_nh, "amax_b");
    limits.arevoffset_b = uf_common::getParam<Eigen::Vector3d>(private_nh, "arevoffset_b");
    limits.umax_b = uf_common::getParam<subjugator::Vector6d>(private_nh, "umax_b");
    traj_dt = uf_common::getParam<ros::Duration>(private_nh, "traj_dt", ros::Duration(0.0001));

    odom_sub = nh.subscribe<Odometry>("odom", 1, boost::bind(&Node::odom_callback, this, _1));

    trajectory_pub = nh.advertise<PoseTwistStamped>("trajectory", 1);
    waypoint_pose_pub = private_nh.advertise<PoseStamped>("waypoint", 1);

    update_timer =
        nh.createTimer(ros::Duration(1. / 50), boost::bind(&Node::timer_callback, this, _1));

    actionserver.start();

    set_disabled_service = private_nh.advertiseService<SetDisabledRequest, SetDisabledResponse>(
        "set_disabled", boost::bind(&Node::set_disabled, this, _1, _2));
  }
예제 #5
0
 followerAction(std::string name) :
   action_(nh_, name, boost::bind(&followerAction::executeCB, this, _1), false),
   action_name_(name)
 {
   action_.start();
     ROS_INFO("Waiting for the Client to start the process");
 }
예제 #6
0
  TestAction(std::string name) :
    as_(nh_, name, boost::bind(&TestAction::executeCB, this, _1), false),
    action_name_(name)
  {
    twist_pub = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 1000);
    dist_sub = nh_.subscribe("/ir_publish/sensors", 1, &TestAction::distanceCallback, this);
    

    distance_lefts_front = init_side_limit_max;
    distance_lefts_back = init_side_limit_max;
    distance_front_left = init_front_limit_max;
    distance_front_right = init_front_limit_max;
    distance_rights_front = init_side_limit_max;
    distance_rights_back = init_side_limit_max;

    // Coresponding to the right decision side
    side = 0;
    flagReady = 0;
    
    srand(time(NULL));

    state = FRONT;
    counter = 0;

    as_.start();
  }
 MotorController(int hz) : ignore_twist(false), ignored_twist_cnt(0), Node(), hz(hz), wheel_left(hz), wheel_right(hz), v(0), w(0), updates_since_last_twist(0), idle(false), is_still(false), stop_action(nh, ACTION_STOP, boost::bind(&MotorController::action_execute_stop_callback, this, _1), false), is_stopping(false) {
     init_params();
     print_params();
     pwm_publisher = nh.advertise<ras_arduino_msgs::PWM>(TOPIC_PWM, BUFFER_SIZE);
     actual_twist_publisher = nh.advertise<geometry_msgs::Twist>(TOPIC_ACTUAL_TWIST, BUFFER_SIZE);
     twist_subscriber = nh.subscribe<geometry_msgs::Twist>(TOPIC_TWIST, BUFFER_SIZE, &MotorController::twist_callback, this);
     encoders_subscriber = nh.subscribe<ras_arduino_msgs::Encoders>(TOPIC_ENCODERS, BUFFER_SIZE, &MotorController::encoders_callback, this);
     stop_action.start();
 }
 PickAndPlaceServer(const std::string name) : 
   nh_("~"), as_(name, false), action_name_(name), client_("/move_arm", true)
 {
 
   //register the goal and feeback callbacks
   as_.registerGoalCallback(boost::bind(&PickAndPlaceServer::goalCB, this));
   as_.registerPreemptCallback(boost::bind(&PickAndPlaceServer::preemptCB, this));
   
   as_.start();
 }
    ExpressionDriver(std::string name, ros::NodeHandle &n) :  nh_(n), nPriv("~"), as_(nh_, name, boost::bind(&ExpressionDriver::executeCB, this, _1), false),
        action_name_()
    {

        //Initialize the port
        nPriv.param<std::string>("port", port, "/dev/ttyACM0");
        nPriv.param<int>("baudrate", baudrate, 115200);

        try
        {
            driverComms = new DriverComms(port, baudrate);
        }
        catch(const std::exception&)
        {
            throw std::exception();
        }

        //Start the action server

        as_.start();


        //Initialize and set the default expression
        bool sentM, sentS, sentR, sentL;

        sentM = driverComms->sendCommand(ExpressionValues::ExpressionHappy::MOUTH, vizzy_expressions::ExpressionActionGoal::_goal_type::PART_MOUTH);
        feedback_.mouth_value = ExpressionValues::ExpressionHappy::MOUTH;
        feedback_.mouth_emotion = vizzy_expressions::ExpressionActionGoal::_goal_type::FACE_HAPPY;

        sentS = driverComms->sendCommand(ExpressionValues::ExpressionHappy::EYELIDS, vizzy_expressions::ExpressionActionGoal::_goal_type::PART_EYELIDS);
        feedback_.eyelids_emotion = vizzy_expressions::ExpressionActionGoal::_goal_type::FACE_HAPPY;
        feedback_.eyelids_value = ExpressionValues::ExpressionHappy::EYELIDS;

        sentR = driverComms->sendCommand(ExpressionValues::ExpressionHappy::RIGHT_EYEBROW, vizzy_expressions::ExpressionActionGoal::_goal_type::PART_RIGHTEYEBROW);
        feedback_.rightEyebrow_emotion = vizzy_expressions::ExpressionActionGoal::_goal_type::FACE_HAPPY;
        feedback_.rightEyebrow_value = ExpressionValues::ExpressionHappy::RIGHT_EYEBROW;

        sentL = driverComms->sendCommand(ExpressionValues::ExpressionHappy::LEFT_EYEBROW, vizzy_expressions::ExpressionActionGoal::_goal_type::PART_LEFTEYEBROW);
        feedback_.leftEyebrow_emotion = vizzy_expressions::ExpressionActionGoal::_goal_type::FACE_HAPPY;
        feedback_.leftEyebrow_value = ExpressionValues::ExpressionHappy::LEFT_EYEBROW;

        if(sentM && sentS && sentR && sentL)
        {
            ROS_INFO("Initialization commands sent. Vizzy is Happy! :D");

        }
        else
        {
            ROS_ERROR("Expression initialization failed. Check the board? :(");
            as_.shutdown();
            delete driverComms;
            throw std::exception();
        }

    }
    MovePlatformAction() :
        as_(n_, CARLOS_MOVE_ACTION, false), //movePlatform action SERVER
        ac_planner_("/plan_action", true), // Planner action CLIENT
        ac_control_("/control_action", true) // Controller action CLIENT
    {

        n_.param("/move_platform_server/debug", debug_, false);

        std::string name = ROSCONSOLE_DEFAULT_NAME; //ros.carlos_motion_action_server
        name = name  + ".debug";
        logger_name_ = "debug";
        //logger is ros.carlos_motion_action_server.debug

        if (debug_)
        {
            // if we use ROSCONSOLE_DEFAULT_NAME we'll get a ton of debug messages from actionlib which is annoying!!!
            // so for debug we'll use a named logger
            if(ros::console::set_logger_level(name, ros::console::levels::Debug)) //name
                ros::console::notifyLoggerLevelsChanged();
        }
        else // if not DEBUG we want INFO
        {
            if(ros::console::set_logger_level(name, ros::console::levels::Info)) //name
                ros::console::notifyLoggerLevelsChanged();
        }

        ROS_DEBUG_NAMED(logger_name_, "Starting Move Platform Server");

        as_.registerGoalCallback(boost::bind(&MovePlatformAction::moveGoalCB, this));
        as_.registerPreemptCallback(boost::bind(&MovePlatformAction::movePreemptCB, this));

        //start the move server
        as_.start();
        ROS_DEBUG_NAMED(logger_name_, "Move Platform Action Server Started");

        // now wait for the other servers (planner + controller) to start
        ROS_WARN_NAMED(logger_name_, "Waiting for planner server to start");
        ac_planner_.waitForServer();
        ROS_INFO_STREAM_NAMED(logger_name_, "Planner server started: " <<  ac_planner_.isServerConnected());

        ROS_WARN_NAMED(logger_name_, "Waiting for controller server to start");
        ac_control_.waitForServer();
        ROS_INFO_STREAM_NAMED(logger_name_, "Controller server started: " <<  ac_control_.isServerConnected());

        n_.param("/carlos/fsm_frequency", frequency_, DEFAULT_STATE_FREQ);
        state_pub_timer_ = n_.createTimer(frequency_, &MovePlatformAction::state_pub_timerCB, this);
        state_pub_ = n_.advertise<mission_ctrl_msgs::hardware_state>(CARLOS_BASE_STATE_MSG,1);

        planning_ = false;
        controlling_ = false;
        //set_terminal_state_;
        ctrl_state_sub = n_.subscribe<std_msgs::String>("/oea_controller/controller_state", 5, &MovePlatformAction::control_stateCB, this);
        planner_state_sub = n_.subscribe<std_msgs::UInt8>("/oea_planner/state", 5, &MovePlatformAction::planner_stateCB, this);

    }
예제 #11
0
 SayAction(std::string name) :
   as_(nh_, name, boost::bind(&SayAction::as_cb, this, _1), false),
   action_name_(name)
 {
   as_.start();
   srvServer_ = nh_.advertiseService("/say", &SayAction::service_cb, this);
   srvServer_mute_ = nh_.advertiseService("mute", &SayAction::service_cb_mute, this);
   srvServer_unmute_ = nh_.advertiseService("unmute", &SayAction::service_cb_unmute, this);
   sub_ = nh_.subscribe("/say", 1000, &SayAction::topic_cb, this);
   topicPub_Diagnostic_ = nh_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
   mute_ = false;
 }
  Trajectory_Tracker(ros::NodeHandle n) :
    action_server(n,ros::this_node::getName(),boost::bind(&Trajectory_Tracker::track_trajectory, this, _1), false),
    action_name(ros::this_node::getName())
    {

      action_server.start();
      ROS_INFO("STARTED ACTION SERVER");
      marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
      cmd_pub = n.advertise<geometry_msgs::Pose2D>("cmd", 1000);
      Kp_x = 1;
      Kp_y = 4;
      Kp_w = 1.75;
    }
예제 #13
0
  SpinBucketAction(std::string name) :
	  as_(nh_, name, boost::bind(&SpinBucketAction::executeCB, this, _1), false),
	  action_name_(name)
  {
	// register the goal and feeback callbacks
	point_sub_ = nh_.subscribe("/track_bucket_point_avg", 1, &SpinBucketAction::trackBucketPointCB, this);
	ir_sub_ = nh_.subscribe("/beacon", 1, &SpinBucketAction::trackIRCB, this);
	twist_pub_ = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
	bucket_detected_=false;
  	ir_detected_=false;
    entered_ir = false;

    as_.start();
  }
  PickAndPlaceServer(const std::string name) :
    nh_("~"), as_(name, false), action_name_(name), client_("/move_right_arm", true)
  {
 
    //register the goal and feeback callbacks
    as_.registerGoalCallback(boost::bind(&PickAndPlaceServer::goalCB, this));
    as_.registerPreemptCallback(boost::bind(&PickAndPlaceServer::preemptCB, this));
   
    as_.start();

    client_.waitForServer();
    ROS_INFO("Connected to server");


    gripper = nh_.advertise<std_msgs::Float64>("/parallel_gripper_controller/command", 1, false);
  }
  ApproachBucketAction(std::string name) :
    as_(nh_, name, false),
    action_name_(name)
  {
	// register the goal and feeback callbacks
    as_.registerGoalCallback(boost::bind(&ApproachBucketAction::goalCB, this));
	as_.registerPreemptCallback(boost::bind(&ApproachBucketAction::preemptCB, this));
	twist_pub_ = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
    us_sub_ = nh_.subscribe("/obstacle", 1, &ApproachBucketAction::ultraSonicsCB, this);
	point_sub_ = nh_.subscribe("track_bucket_point_avg", 1, &ApproachBucketAction::trackPointCB, this);
	area_sub_ = nh_.subscribe("track_bucket_area", 1, &ApproachBucketAction::trackAreaCB, this);
	obstacle_detected_=false;
	drop_flag = false;
	
    as_.start();
  }
    DynamixelProActionServer(std::string name): _actionServer(_nodeHandle, name, boost::bind(&DynamixelProActionServer::goalCallback, this, _1) ,false) {
        _actionServer.registerPreemptCallback(boost::bind(&DynamixelProActionServer::preemptCallback, this));
        if(_nodeHandle.getParam("joint_name_list", _jointInfo.name)) {
            init();
            _publishFeedback = false;
            _jointState = _nodeHandle.subscribe("joint_states", 100, &DynamixelProActionServer::jointStateCallback,
                                                this);
            _jointCommand = _nodeHandle.advertise<sensor_msgs::JointState>("joint_commands", 10);
            _actionServer.start();
            rosInfo("Server is active");
        }
        else {
            rosError("Cloud not find joint_name_list parameter");
            ros::shutdown();
        }

    }
 MoveActionServer(const std::string name) : 
   nh_("~"), as_(nh_, name, false), action_name_(name)
 {
   // Get parameters
   nh_.param<std::string>("base_frame", base_frame, "base_link");
   nh_.param<std::string>("odom_frame", odom_frame, "odom");
   nh_.param<double>("turn_rate", turn_rate, 0.75);
   nh_.param<double>("forward_rate", forward_rate, 0.25);
   
   //register the goal and feeback callbacks
   as_.registerGoalCallback(boost::bind(&MoveActionServer::goalCB, this));
   as_.registerPreemptCallback(boost::bind(&MoveActionServer::preemptCB, this));
   
   as_.start();
   
   cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
 }
  /**
   * @brief This action server uses the softMotion-libs to move the robot
   *        along a main trajectory described in its goal.
   *        It correct errors by computing small trajectories to correct
   *        its motion and stick to the main trajectory.
  **/
  SoftMoveBaseAction(std::string name) :
    as_(nh_, name, boost::bind(&SoftMoveBaseAction::executeCB, this, _1), false),
    action_name_(name)
  {
    as_.start();
    _dt = 0.1;
    //_tmpTrajDt = 1.0;
    _timeFromStart = 0;
    _tmpTimeFromStart = 0;
    _timer = 0;
    _tolerance[0] = 0.01;
    _tolerance[1] = 0.01;
    _tolerance[2] = 0.5;
    // last velocity and acceleration sent init
    for(int i =0; i<6; ++i)
    {
      _lastVel[i] = 0.0;
      _lastAcc[i] = 0.0;
    }

    _nextRobotCond_r.resize(6);
    _newRobotCond_r.resize(6);
    _currRobotCond_r.resize(6);

    // initialize the limits, there are updated for 
    // each new received trajectory
    for(int i=0; i<6; ++i)
    {
      limit.maxJerk = 0.9;
      limit.maxAcc =  0.3;
      limit.maxVel =  0.6;
      limits.push_back(limit);
    }

    //wait for the listener to get the first message
    _listener.waitForTransform("map", "base_footprint", 
                               ros::Time(0), ros::Duration(1.0));

    //set up the publisher for the cmd_vel topic
    cmd_vel_pub = nh_.advertise<geometry_msgs::Twist>("base_controller/command", 1);
  
    timeScaleSub_ = nh_.subscribe<std_msgs::Float64>("soft_move_base/timescale", 1, &SoftMoveBaseAction::timeScaleCB, this);
    timeScale_ = 1;

    ROS_INFO("soft_move_base action server up and ready");
  }
예제 #19
0
  TabletopGraspActionServer(std::string name) :
	as_(nh_, name, boost::bind(&TabletopGraspActionServer::executeCB, this, _1), false),
    action_name_(name)
  {
	
	
	
	heardPose = false;
	heardJoinstState = false;
	heardGrasps = false;

	//create subscriber to joint angles
	sub_angles = nh_.subscribe ("/joint_states", 1, &TabletopGraspActionServer::joint_state_cb, this);


	//create subscriber to joint torques
	sub_torques = nh_.subscribe ("/mico_arm_driver/out/joint_efforts", 1, &TabletopGraspActionServer::joint_effort_cb,this);

	//create subscriber to tool position topic
	sub_tool = nh_.subscribe("/mico_arm_driver/out/tool_position", 1, &TabletopGraspActionServer::toolpos_cb, this);

	//subscriber for fingers
	sub_finger = nh_.subscribe("/mico_arm_driver/out/finger_position", 1, &TabletopGraspActionServer::fingers_cb, this);
	  
	//subscriber for grasps
	sub_grasps = nh_.subscribe("/find_grasps/grasps_handles",1, &TabletopGraspActionServer::grasps_cb,this);  
	  
	//publish velocities
	pub_velocity = nh_.advertise<geometry_msgs::TwistStamped>("/mico_arm_driver/in/cartesian_velocity", 10);
	
	//publish pose array
	pose_array_pub = nh_.advertise<geometry_msgs::PoseArray>("/agile_grasp_demo/pose_array", 10);
	
	//publish pose 
	pose_pub = nh_.advertise<geometry_msgs::PoseStamped>("/agile_grasp_demo/pose_out", 10);
	pose_fk_pub = nh_.advertise<geometry_msgs::PoseStamped>("/agile_grasp_demo/pose_fk_out", 10);
	
	//debugging publisher
	cloud_pub = nh_.advertise<sensor_msgs::PointCloud2>("agile_grasp_demo/cloud_debug", 10);
	cloud_grasp_pub = nh_.advertise<sensor_msgs::PointCloud2>("agile_grasp_demo/cloud", 10);
	
    ROS_INFO("Starting grasp action server...");
    
    as_.start();
  }
예제 #20
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);
            
        }
예제 #21
0
    spin_degreesAction(std::string name) :

    // action server
    as_(nh_, name, boost::bind(&spin_degreesAction::executeCB, this, _1), false),
    action_name_(name)
  {
        // Subscribers and publishers

    imu_sub_ = nh_.subscribe(IMU_SUBSCRIBER, 1, &spin_degreesAction::imu_callback, this);
    cmd_vel_pub_=nh_.advertise<geometry_msgs::TwistStamped>(cmd_vel_PUBLISHER,1);



    counter_=0;
    success_=false;
    
    as_.start();
  }
	PLAN2CTRLAction(std::string name) :
		as_(nh_, name, boost::bind(&PLAN2CTRLAction::executeCB, this, _1), false),
		action_name_(name)
{
		ee_ft.resize(6);
		// ROS TOPICS for controllers
		sub_ = nh_.subscribe<geometry_msgs::PoseStamped>(EE_STATE_POSE_TOPIC, 1, eeStateCallback);
		sub_ft_ = nh_.subscribe<geometry_msgs::WrenchStamped>(EE_STATE_FT_TOPIC, 1, eeFTCallback);
		pub_ = nh_.advertise<geometry_msgs::PoseStamped>(EE_CMD_POSE_TOPIC, 1);
		pub_ft_ = nh_.advertise<geometry_msgs::WrenchStamped>(EE_CMD_FT_TOPIC, 1);
		pub_gmr_out_ = nh_.advertise<std_msgs::Float32>("GMR_OUT", 1);
	        pub_action_state_ = nh_.advertise<std_msgs::String>("Action_State", 1000);
		pub_action_name_ = nh_.advertise<std_msgs::String>("/motion_planner/action_name", 1000);
		pub_model_fname_ = nh_.advertise<std_msgs::String>("/motion_planner/model_file_name", 1000);
		pub_plot_dyn_ = nh_.advertise<std_msgs::Int32>("/motion_planner/plot_dynamics", 1000);
		pub_ee_pos_att_ = nh_.advertise<geometry_msgs::PoseStamped>("/motion_planner/ee_pos_att", 1000);
		pub_ee_ft_att_ = nh_.advertise<geometry_msgs::WrenchStamped>("/motion_planner/ee_ft_att", 1000);
		//sub_plot_pub_ = nh_.subscribe<std_msgs::Int32>("/motion_planner/plot_published", 10, ppCallback);
		as_.start();
}
예제 #23
0
  BTAction(std::string name) :
    as_(nh_, name, boost::bind(&BTAction::executeCB, this, _1), false),
    action_name_(name)
  {
    as_.start();
    robot_ip = "192.168.0.188";
    std::cout << "Robot ip to use is: " << robot_ip << std::endl;
    motion_proxy_ptr = new AL::ALMotionProxy("192.168.0.188", 9559);


    //put this in init()

    AL::ALValue stiffness_name("Body");
    AL::ALValue stiffness(1.0f);
    AL::ALValue stiffness_time(1.0f);
    //motion_proxy_ptr->stiffnessInterpolation(stiffness_name,
                                           //  stiffness,
                                            // stiffness_time);
    //motion_proxy_ptr->moveInit();

  }
예제 #24
0
	Explore(std::string name):
		as_(nh_, name, false),
		action_name_(name),
		ac_("move_base", true)
	{
		explore_state_ = STOP;
		explore_ = false;
		firstGoalSend = false;
		
		as_.registerGoalCallback(boost::bind(&Explore::goalCB, this));
		as_.registerPreemptCallback(boost::bind(&Explore::preemptCB, this));

		as_.start();

		alghoritm_state_sub_ =   nh_.subscribe("alghoritm_state", 1, &Explore::alghoritmStateCallBack, this);
		posSub = nh_.subscribe("/amcl_pose", 1, &Explore::poseCallback, this);
		deadlock_service_state_sub = nh_.subscribe("/deadlock_service_state", 1, &Explore::deadlockServiceStateCb, this);


		
	}
예제 #25
0
파일: node.cpp 프로젝트: uf-mil/SubjuGator
  Node()
    : private_nh("~")
    , actionserver(nh, "moveto", false)
    , disabled(false)
    , kill_listener(nh, "kill")
    , waypoint_validity_(nh)
  {
    // Make sure alarm integration is ok
    kill_listener.waitForUpdate(ros::Duration(10));
    if (kill_listener.getNumConnections() < 1)
      throw std::runtime_error("The kill listener isn't connected to the alarm server");
    kill_listener.start();  // F**k.

    fixed_frame = mil_tools::getParam<std::string>(private_nh, "fixed_frame");
    body_frame = mil_tools::getParam<std::string>(private_nh, "body_frame");

    limits.vmin_b = mil_tools::getParam<subjugator::Vector6d>(private_nh, "vmin_b");
    limits.vmax_b = mil_tools::getParam<subjugator::Vector6d>(private_nh, "vmax_b");
    limits.amin_b = mil_tools::getParam<subjugator::Vector6d>(private_nh, "amin_b");
    limits.amax_b = mil_tools::getParam<subjugator::Vector6d>(private_nh, "amax_b");
    limits.arevoffset_b = mil_tools::getParam<Eigen::Vector3d>(private_nh, "arevoffset_b");
    limits.umax_b = mil_tools::getParam<subjugator::Vector6d>(private_nh, "umax_b");
    traj_dt = mil_tools::getParam<ros::Duration>(private_nh, "traj_dt", ros::Duration(0.0001));

    waypoint_check_ = mil_tools::getParam<bool>(private_nh, "waypoint_check");

    odom_sub = nh.subscribe<Odometry>("odom", 1, boost::bind(&Node::odom_callback, this, _1));

    trajectory_pub = nh.advertise<PoseTwistStamped>("trajectory", 1);
    trajectory_vis_pub = private_nh.advertise<PoseStamped>("trajectory_v", 1);
    waypoint_pose_pub = private_nh.advertise<PoseStamped>("waypoint", 1);

    update_timer = nh.createTimer(ros::Duration(1. / 50), boost::bind(&Node::timer_callback, this, _1));

    actionserver.start();

    set_disabled_service = private_nh.advertiseService<SetDisabledRequest, SetDisabledResponse>(
        "set_disabled", boost::bind(&Node::set_disabled, this, _1, _2));
  }
	Explore(std::string name):
		as_(nh_, name, false),
		action_name_(name),
		ac_("move_base", true)
	{

		as_.registerGoalCallback(boost::bind(&Explore::goalCB, this));
		as_.registerPreemptCallback(boost::bind(&Explore::preemptCB, this));

		as_.start();
		firstGoalSend = false;

		//all_balls = nh_.subscribe < geometry_msgs::PoseArray > ("allBalls", 10, &ChooseAccessibleBalls::allBallsCb, this);

		alghoritm_state_sub_ =   nh_.subscribe("alghoritm_state", 1, &Explore::alghoritmStateCallBack, this);
		posSub = nh_.subscribe("/amcl_pose", 1, &Explore::poseCallback, this);
		deadlock_service_state_sub = nh_.subscribe("/deadlock_service_state", 1, &Explore::deadlockServiceStateCb, this);


		explore_state_ = STOP;
		explore_ = false;
	}
  FaceRecognition(std::string name) :
    _as(_nh, name, boost::bind(&FaceRecognition::executeCB, this, _1), false),
    _it(_nh),
    _ph("~"),
    _action_name(name),
    _fd(),
    _fr(NULL),
    _fc(NULL),
    _windowName("FaceRec"),
    _window_rows(0),
    _window_cols(0)
  {
    _as.start();

    // Subscrive to input video feed and publish output video feed
    // _image_sub = _it.subscribe("/camera/image_raw", 1, &FaceRecognition::imageCb, this);
    // _image_sub = _it.subscribe("/usb_cam/image_raw", 1, &FaceRecognition::imageCb, this);
    
    // Get params
    _ph.param<std::string>("algorithm", _recognitionAlgo , "lbph");
    _ph.param<int>("no_training_images", _noTrainingImgs , 20);
    _ph.param<int>("max_faces", _maxFaces , 2);
    _ph.param<bool>("display_window", _displayWindow, true);
    _ph.param<string>("preprocessing", _preprocessing, "tantriggs");

    try {
      _fr = new FaceRec(true, _preprocessing);
      ROS_INFO("Using %s for recognition.", _recognitionAlgo.c_str());
      ROS_INFO("Using %s for preprocessing.", _preprocessing.c_str());
    }
    catch( cv::Exception& e ) {
      const char* err_msg = e.what();
      ROS_INFO("%s", err_msg);
      ROS_INFO("Only ADD_IMAGES mode will work. Please add more subjects for recognition.");
    }

    cv::namedWindow(_windowName);
  }
	GoToSelectedBall(std::string name) :
		as_(nh_, name, boost::bind(&GoToSelectedBall::executeCB, this, _1), false),
		action_name_(name),
		ac("move_base", true)
	{
		selected_ball_sub_ = nh_.subscribe < geometry_msgs::Point > ("/one_selected_ball", 1, &GoToSelectedBall::selectedBallCb, this);
		hoover_state_pub_ = nh_.advertise<std_msgs::Int16> ("hoover_state",1);
		go_forward_robot_pub_ = nh_.advertise< std_msgs::Float32>("/robot_go_straight",1);
		go_forward_robot_state_sub_ = nh_.subscribe("/robot_go_straight_state", 1, &GoToSelectedBall::robotGoStraightStateCb, this);
		alghoritm_state_pub_ = nh_.advertise<std_msgs::String> ("/alghoritm_state",1);
		deadlock_service_state_sub = nh_.subscribe("/deadlock_service_state", 1, &GoToSelectedBall::deadlockServiceStateCb, this);


		firstGoalSent = false;
		isBallPoseSet = false;
		moveStraightState = 0;
		moveStraightStateChange = false;
		is_deadlock_service_run = false;

		as_.start();
		state_ = STOP;

	}
    WaitUntilUnblockedAction (std::string name) :
      as_(nh_, name, boost::bind(&WaitUntilUnblockedAction::executeCB, this, _1), false),
      action_name_(name)
    {
      // Point head
      //Initialize the client for the Action interface to the head controller
      point_head_client_ = new
        PointHeadClient("/head_traj_controller/point_head_action", true);

      //wait for head controller action server to come up 
      while(!point_head_client_->waitForServer(ros::Duration(5.0)))
      {
        ROS_INFO("Waiting for the point_head_action server to come up");
      }

      // human detection
      activated = false;
      human_detected = false;
      obstacle_detected = false;

      marker_pub = nh_.advertise<visualization_msgs::Marker>("obstacle_boundingbox", 1);

      as_.start();
    }
예제 #30
0
 FibonacciAction(std::string name)
     : as_(nh_, name, boost::bind(&FibonacciAction::executeCB, this, _1),
           false),
       action_name_(name) {
   as_.start();
 }