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

    }
  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();
  }
  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);
  }
    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);
 }
예제 #8
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);


		
	}
	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;
	}
  // Constructor
  PickPlaceMoveItServer(const std::string name):
    nh_("~"),
    movegroup_action_("pickup", true),
    clam_arm_client_("clam_arm", true),
    ee_marker_is_loaded_(false),
    block_published_(false),
    action_server_(name, false)
  {
    base_link_ = "/base_link";

    // -----------------------------------------------------------------------------------------------
    // Adding collision objects
    collision_obj_pub_ = nh_.advertise<moveit_msgs::CollisionObject>(COLLISION_TOPIC, 1);

    // -----------------------------------------------------------------------------------------------
    // Connect to move_group/Pickup action server
    while(!movegroup_action_.waitForServer(ros::Duration(4.0))){ // wait for server to start
      ROS_INFO_STREAM_NAMED("pick_place_moveit","Waiting for the move_group/Pickup action server");
    }

    // ---------------------------------------------------------------------------------------------
    // Connect to ClamArm action server
    while(!clam_arm_client_.waitForServer(ros::Duration(5.0))){ // wait for server to start
      ROS_INFO_STREAM_NAMED("pick_place_moveit","Waiting for the clam_arm action server");
    }

    // ---------------------------------------------------------------------------------------------
    // Create planning scene monitor
    planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor(ROBOT_DESCRIPTION));

    if (planning_scene_monitor_->getPlanningScene())
    {
      //planning_scene_monitor_->startWorldGeometryMonitor();
      //planning_scene_monitor_->startSceneMonitor("/move_group/monitored_planning_scene");
      //planning_scene_monitor_->startStateMonitor("/joint_states", "/attached_collision_object");
    }
    else
    {
      ROS_FATAL_STREAM_NAMED("pick_place_moveit","Planning scene not configured");
    }

    // ---------------------------------------------------------------------------------------------
    // Load the Robot Viz Tools for publishing to Rviz
    rviz_tools_.reset(new block_grasp_generator::RobotVizTools(RVIZ_MARKER_TOPIC, EE_GROUP, PLANNING_GROUP_NAME, base_link_, planning_scene_monitor_));

    // ---------------------------------------------------------------------------------------------
    // Register the goal and preempt callbacks
    action_server_.registerGoalCallback(boost::bind(&PickPlaceMoveItServer::goalCB, this));
    action_server_.registerPreemptCallback(boost::bind(&PickPlaceMoveItServer::preemptCB, this));
    action_server_.start();

    // Announce state
    ROS_INFO_STREAM_NAMED("pick_place_moveit", "Server ready.");
    ROS_INFO_STREAM_NAMED("pick_place_moveit", "Waiting for pick command...");

    // ---------------------------------------------------------------------------------------------
    // Send home
    ROS_INFO_STREAM_NAMED("pick_place_moveit","Sending home");
    clam_arm_goal_.command = clam_msgs::ClamArmGoal::RESET;
    clam_arm_client_.sendGoal(clam_arm_goal_);
    while(!clam_arm_client_.getState().isDone() && ros::ok())
      ros::Duration(0.1).sleep();

    // ---------------------------------------------------------------------------------------------
    // Send fake command
    fake_goalCB();
  }
예제 #11
0
  HueDetector3DServer(std::string s_name) :
    server_(n_, s_name), action_name_(s_name) {
    
    first = true;
    
    if (!n_.getParam("HueServer/maxNumComponents", maxNumComponents)) {
      ROS_INFO("Could not retrieve 'maxNumComponents' parameter, setting to default value of '%d'", MAX_FEATURES);
      maxNumComponents = MAX_FEATURES;
    }

    if (!n_.getParam("HueServer/blurKernel", blurKernel)) {
      ROS_INFO("Could not retrieve 'blurKernel' parameter, setting to default value of '%d'", init_blur_kernel);
      blurKernel = init_blur_kernel;
    }

    if (!n_.getParam("HueServer/minCCThreshold", minCCThreshold)) {
      ROS_INFO("Could not retrieve 'minCCThreshold' parameter, setting to default value of '%d'", DEFAULT_MIN_CC_THRESHOLD);
      minCCThreshold = DEFAULT_MIN_CC_THRESHOLD;
    }

    if (!n_.getParam("HueServer/maxCCThreshold", maxCCThreshold)) {
      ROS_INFO("Could not retrieve 'maxCCThreshold' parameter, setting to default value of '%d'", DEFAULT_MAX_CC_THRESHOLD);
      maxCCThreshold = DEFAULT_MAX_CC_THRESHOLD;
    }
    
    if (!n_.getParam("HueServer/display_image", display_image)) {
      ROS_INFO("Could not retrieve 'display_image' parameter, setting to default value of '%d'", init_display_image);
      display_image = init_display_image;
    }	

    std::string source_identifier;
    if (!n_.getParam("HueServer/source_identifier", source_identifier)) {
      ROS_INFO("Could not retrieve 'source_identifier' parameter, setting to default value of '%s'", DEFAULT_SOURCE_IDENTIFIER);
      source_identifier = DEFAULT_SOURCE_IDENTIFIER;
    }
  
    std::stringstream hue_name;
    hue_name << name << "HueServer";
    sprintf(pointcloud_topic, "%s", source_identifier.c_str());	//input topic
    sprintf(color_topic, "%s/Feature/ColorBlobs/%s", source_identifier.c_str(), hue_name.str().c_str());		
    srand ( time(NULL) );

    //output topic
    sprintf(marker_topic,"ellipsoid_marker/%s",hue_name.str().c_str());
    printf("Topic name : %s\n", color_topic);
    pointcloud_sub_ = n_.subscribe(pointcloud_topic, 1, &HueDetector3DServer::pointCloudCallback, this);
    //moments_pub_ = n_.advertise<Image_msgs::FeatureMoments3D>(color_topic, 1);
    object_pose_marker_pub_ = n_.advertise<visualization_msgs::Marker>(marker_topic,1);
    
    if (display_image) {
      namedWindow(name, CV_WINDOW_AUTOSIZE);
      //namedWindow("feat", CV_WINDOW_AUTOSIZE);
      createTrackbar("Blur Kernel", name, &blurKernel, 25, &on_trackbar);
      createTrackbar("Min Size", name, &minCCThreshold, 3000, &on_trackbar); 
      createTrackbar("Max Size", name, &maxCCThreshold, 10000, &on_trackbar);
    }
   	
    hue_image = new Mat(height, width, CV_8UC1);
    back_img = new Mat(hue_image->size(), CV_8UC1);
    temp_blurred_image = new Mat(hue_image->size(), CV_8UC1);
    blurred_image = new Mat(hue_image->size(), CV_8UC1);
    flipped = new Mat(hue_image->size(), CV_8UC1);
    rgb_image = new Mat(hue_image->size(), CV_8UC3);
    hsv_image = new Mat(hue_image->size(), CV_8UC3);
    
    is_running = false;
    got_goals = false;
    num_feature_goals_ = 0;
    timer_ = n_.createTimer(ros::Duration(1.0 / update_rate), &HueDetector3DServer::Update, this);
    //register the goal and feeback callbacks
    server_.registerGoalCallback(boost::bind(&HueDetector3DServer::GoalCB, this));
    server_.registerPreemptCallback(boost::bind(&HueDetector3DServer::PreemptCB, this));
    ROS_INFO("Hue Server started ");
  }