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);
}
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);
}
}
/**
* 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;
}
