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();
}
void RosAriaNode::sonarConnectCb()
{
robot->lock();
if (sonar_pub.getNumSubscribers() == 0)
{
robot->disableSonar();
use_sonar = false;
}
else
{
robot->enableSonar();
use_sonar = true;
}
robot->unlock();
}
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());
}
/// 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;
}
