Пример #1
0
void RosAriaNode::sonarConnectCb()
{
  if (!robot->tryLock()) {
    ROS_ERROR("Skipping sonarConnectCb because could not lock");
    return;
  }
  if (!robot->areSonarsEnabled())
  {
    robot->enableSonar();
    sonar_tf_timer.start();
  }
  robot->unlock();
}
Пример #2
0
void RosAriaNode::sonarConnectCb()
{
  robot->lock();
  if (sonar_pub.getNumSubscribers() == 0)
  {
    robot->disableSonar();
    use_sonar = false;
  }
  else
  {
    robot->enableSonar();
    use_sonar = true;
  }
  robot->unlock();
}
Пример #3
0
void SetupRobot(void)
{
	puts("attempting to connect to robot");
	RobotConnectoin.setPort("COM8");
	RobotConnectoin.setBaud(9600);
	robot.setDeviceConnection(&RobotConnectoin);
	if(!robot.blockingConnect()){puts("not connected to robot");Aria::shutdown();}
	robot.addRangeDevice(&sonarDev);
	robot.addRangeDevice(&bumpers);
	robot.enableMotors();
	robot.enableSonar();
	robot.requestEncoderPackets();
	robot.setCycleChained(false);
//	robot.setRotVelMax(robot.getRotVelMax());
}
Пример #4
0
/// Called when another node subscribes or unsubscribes from sonar topic.
void RosAriaNode::sonarConnectCb()
{
  publish_sonar = (sonar_pub.getNumSubscribers() > 0);
  publish_sonar_pointcloud2 = (sonar_pointcloud2_pub.getNumSubscribers() > 0);
  robot->lock();
  if (publish_sonar || publish_sonar_pointcloud2)
  {
    robot->enableSonar();
    sonar_enabled = false;
  }
  else if(!publish_sonar && !publish_sonar_pointcloud2)
  {
    robot->disableSonar();
    sonar_enabled = true;
  }
  robot->unlock();
}
int RosAriaNode::Setup()
{
	ArArgumentBuilder *args;

	args = new ArArgumentBuilder();
	args->add("-rp"); //pass robot's serial port to Aria
	args->add(serial_port.c_str());
	conn = new ArSimpleConnector(args);


	robot = new ArRobot();
	robot->setCycleTime(1);

	ArLog::init(ArLog::File, ArLog::Verbose, "aria.log", true);

	//parse all command-line arguments
	if (!Aria::parseArgs())
	{
		Aria::logOptions();
		return 1;
	}

	// Connect to the robot
	if (!conn->connectRobot(robot)) {
		ArLog::log(ArLog::Terse, "rotate: Could not connect to robot! Exiting.");
		return 1;
	}

	//Sonar sensor
	sonar.setMaxRange(Max_Range);
	robot->addRangeDevice(&sonar);
	robot->enableSonar();

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

	robot->runAsync(true);

	return 0;
}