Beispiel #1
0
int main(int argc, char **argv)
{
	setFreqs(0,0);

	init(argc,argv,"motors");
	NodeHandle n;

	std::string onoff;
	if(argc > 1)
		onoff = argv[1];

	setPower(onoff == "on");

	signal(SIGINT, onSigint);

	last_cmdvel = Time::now();
	cur_time = Time::now();
	send_time = Time::now();

	ServiceServer srv_on = n.advertiseService("motor_on", callbackOn);
	ServiceServer srv_off = n.advertiseService("motor_off", callbackOff);
	ServiceServer srv_tm = n.advertiseService("timed_motion", callbackTimedMotion); 
	
	Subscriber sub_raw = n.subscribe("motor_raw", 10, callbackRaw);
	Subscriber sub_cmdvel = n.subscribe("cmd_vel", 10, callbackCmdvel);
	Subscriber sub_9axis = n.subscribe("/imu/data_raw", 10, callback9Axis);

	Publisher pub_odom = n.advertise<nav_msgs::Odometry>("odom", 10);
	odom_x = 0.0;
	odom_y = 0.0;
	odom_theta = 0.0;

	send_time = Time::now();

	Rate loop_rate(10);
	while(ok()){
		if(in_cmdvel and Time::now().toSec() - last_cmdvel.toSec() >= 1.0)
			setFreqs(0,0);

		pub_odom.publish(send_odom());
		spinOnce();
		loop_rate.sleep();
	}
	
	exit(0);
}
Beispiel #2
0
int main(int argc,char**argv)
{
    init(argc,argv,"map_node");
    NodeHandle n;
    int _nmarker;
    Point2f* prtm=returnArray (_nmarker);
    Map m(n,prtm,_nmarker);
    Subscriber pos_sub=n.subscribe(ESTIMATED_POSE,100,&Map::updateVehiclePosition,&m);
    ros::ServiceServer  _server(n.advertiseService(string(SERVICE_NAME),&Map::getObjectLocation,&m ));
    ros::spin();
    return 0;

}
MechanismManagerServer::MechanismManagerServer(MechanismManagerInterface* mm_interface, NodeHandle& nh)
    : spinner_ptr_(NULL)
{
    assert(mm_interface!=NULL);

    mm_interface_ = mm_interface;

    if(master::check())
    {
        ss_ = nh.advertiseService("mechanism_manager_interaction",
                                  &MechanismManagerServer::CallBack, this);

        spinner_ptr_ = new AsyncSpinner(1); // Use one thread to keep the ros magic alive
        spinner_ptr_->start();
    }
    else
    {
        std::string err("Can not start the action server, did you start roscore?");
        throw std::runtime_error(err);
    }
}
Beispiel #4
0
/**
 * The main method that starts MAESTOR! This is where the ros node is made and all of the 
 * services are advertised. There are also wrappers in here for all of the service methods that 
 * link up to the ones that are in RobotControl. 
 * @param  argc Number of arguments
 * @param  argv Argument array
 * @return      0 when it shuts down
 */
int main(int argc, char **argv) {
    ros::init(argc, argv, "Maestor"); 
    setRealtime();

    //Check for the run type

    if(argc == 2)
    {
        if(strcmp(argv[1], "sim") == 0)
        {
            //If simulation set the runtime to sim
            robot.setSimType();
        }
    }
    //Init the node
    NodeHandle n; //Fully initializes the node
    Scheduler timer(FREQ_200HZ);
    robot.setPeriod(1.0/timer.getFrequency());
    ServiceServer srv = n.advertiseService("fib", &fib);
    ServiceServer Initsrv = n.advertiseService("initRobot", &initRobot);
    ServiceServer SPsrv = n.advertiseService("setProperties", &setProperties);
    ServiceServer Comsrv = n.advertiseService("command", &command);
    ServiceServer RMsrv = n.advertiseService("requiresMotion", &requiresMotion);
    ServiceServer GPsrv = n.advertiseService("getProperties", &getProperties);
    ServiceServer LTsrv = n.advertiseService("loadTrajectory", &loadTrajectory);
    ServiceServer IFsrv = n.advertiseService("ignoreFrom", &ignoreFrom);
    ServiceServer IAFsrv = n.advertiseService("ignoreAllFrom", &ignoreAllFrom);
    ServiceServer UFsrv = n.advertiseService("unignoreFrom", &unignoreFrom);
    ServiceServer UAFsrv = n.advertiseService("unignoreAllFrom", &unignoreAllFrom);
    ServiceServer STsrv = n.advertiseService("setTrigger", &setTrigger);
    ServiceServer ETsrv = n.advertiseService("extendTrajectory", &extendTrajectory);
    ServiceServer StTsrv = n.advertiseService("startTrajectory", &startTrajectory);
    ServiceServer SpTsrv = n.advertiseService("stopTrajectory", &stopTrajectory);

    ServiceServer SetPropsrv = n.advertiseService("setProperty", &setProperty);

    while (ros::ok()) {
        ros::spinOnce();
        robot.updateHook();
        timer.sleep();
        timer.update();
    }
    return 0;
}
RobotNavigator::RobotNavigator()
{	
	NodeHandle robotNode;

	std::string serviceName;
	robotNode.param("map_service", serviceName, std::string("get_map"));
	mGetMapClient = robotNode.serviceClient<nav_msgs::GetMap>(serviceName);

	mCommandPublisher = robotNode.advertise<nav2d_operator::cmd>("cmd", 1);
	mStopServer = robotNode.advertiseService(NAV_STOP_SERVICE, &RobotNavigator::receiveStop, this);
	mPauseServer = robotNode.advertiseService(NAV_PAUSE_SERVICE, &RobotNavigator::receivePause, this);
	mCurrentPlan = NULL;

	NodeHandle navigatorNode("~/");
	mPlanPublisher = navigatorNode.advertise<nav_msgs::GridCells>("plan", 1);
	mMarkerPublisher = navigatorNode.advertise<visualization_msgs::Marker>("markers", 1, true);
	
	// Get parameters
	navigatorNode.param("map_inflation_radius", mInflationRadius, 1.0);
	navigatorNode.param("robot_radius", mRobotRadius, 0.3);
	navigatorNode.param("exploration_strategy", mExplorationStrategy, std::string("NearestFrontierPlanner"));
	navigatorNode.param("navigation_goal_distance", mNavigationGoalDistance, 1.0);
	navigatorNode.param("navigation_goal_angle", mNavigationGoalAngle, 1.0);
	navigatorNode.param("exploration_goal_distance", mExplorationGoalDistance, 3.0);
	navigatorNode.param("navigation_homing_distance", mNavigationHomingDistance, 3.0);
	navigatorNode.param("min_replanning_period", mMinReplanningPeriod, 3.0);
	navigatorNode.param("max_replanning_period", mMaxReplanningPeriod, 1.0);
	mCostObstacle = 100;
	mCostLethal = (1.0 - (mRobotRadius / mInflationRadius)) * (double)mCostObstacle;

	robotNode.param("map_frame", mMapFrame, std::string("map"));
	robotNode.param("robot_frame", mRobotFrame, std::string("robot"));
	robotNode.param("robot_id", mRobotID, 1);
	robotNode.param("move_action_topic", mMoveActionTopic, std::string(NAV_MOVE_ACTION));
	robotNode.param("explore_action_topic", mExploreActionTopic, std::string(NAV_EXPLORE_ACTION));
	robotNode.param("getmap_action_topic", mGetMapActionTopic, std::string(NAV_GETMAP_ACTION));
	robotNode.param("localize_action_topic", mLocalizeActionTopic, std::string(NAV_LOCALIZE_ACTION));

	// Apply tf_prefix to all used frame-id's
	mRobotFrame = mTfListener.resolve(mRobotFrame);
	mMapFrame = mTfListener.resolve(mMapFrame);

	try
	{
		mPlanLoader = new PlanLoader("nav2d_navigator", "ExplorationPlanner");
		mExplorationPlanner = mPlanLoader->createInstance(mExplorationStrategy);
		ROS_INFO("Successfully loaded exploration strategy [%s].", mExplorationStrategy.c_str());

		mExploreActionServer = new ExploreActionServer(mExploreActionTopic, boost::bind(&RobotNavigator::receiveExploreGoal, this, _1), false);
		mExploreActionServer->start();
	}
	catch(pluginlib::PluginlibException& ex)
	{
		ROS_ERROR("Failed to load exploration plugin! Error: %s", ex.what());
		mExploreActionServer = NULL;
		mPlanLoader = NULL;
	}

	// Create action servers
	mMoveActionServer = new MoveActionServer(mMoveActionTopic, boost::bind(&RobotNavigator::receiveMoveGoal, this, _1), false);
	mMoveActionServer->start();
	
	mLocalizeActionServer = new LocalizeActionServer(mLocalizeActionTopic, boost::bind(&RobotNavigator::receiveLocalizeGoal, this, _1), false);
	mLocalizeActionServer->start();
	
	if(mRobotID == 1)
	{
		mGetMapActionServer = new GetMapActionServer(mGetMapActionTopic, boost::bind(&RobotNavigator::receiveGetMapGoal, this, _1), false);
		mGetMapActionServer->start();
	}else
	{
		mGetMapActionServer = NULL;
	}
	
	mHasNewMap = false;
	mIsStopped = false;
	mIsPaused = false;
	mStatus = NAV_ST_IDLE;
	mCellInflationRadius = 0;
}