MainWindow::MainWindow(NodeHandle& nh, QWidget *parent) :
QMainWindow(parent),
ui(new Ui::MainWindow)
{
ui->setupUi(this);
startTimer(1000);
// Create model
guides_model_ = new GuidesModel(nh,this);
ui->listView->setModel(guides_model_);
// Add additional feature so that
// we can manually modify the data in ListView
// It may be triggered by hitting any key or double-click etc.
ui->listView->setEditTriggers(QAbstractItemView::AnyKeyPressed |
QAbstractItemView::DoubleClicked );
sub_ = new Subscriber(nh.subscribe("rosout", 1000, &MainWindow::loggerCallback, this));
spinner_ptr_ = new AsyncSpinner(1); // Use one thread to keep the ros magic alive
spinner_ptr_->start();
// TextBox is read only
ui->consoleText->setReadOnly(true);
ui->consoleText->setTextInteractionFlags(0); //0 == Qt::TextInteractionFlag::NoTextInteraction
//QColor color = QColorDialog::getColor(Qt::white,this); // in here your color pallete will open..
QPalette p = ui->consoleText->palette(); // define pallete for textEdit..
p.setColor(QPalette::Base, Qt::black); // set color "Red" for textedit base
//p.setColor(QPalette::Text, Qt::white); // set text color which is selected from color pallete
ui->consoleText->setPalette(p); // change textedit palette
// Explanation of this hack:
// Qt and Ros don't like each other, or better, their respective thread workers don't like each other.
// You should not handle qt objects with ROS threads.
// So we use the ros callback to trigger the qt callback :)
QObject::connect(this, SIGNAL(requestUpdateConsole(const QString&,int)),
this, SLOT(updateConsole(const QString&,int)));
// SlideBar
slidebar_res_ = 100;
ui->mergeSlider->setMinimum(0);
ui->mergeSlider->setMaximum(slidebar_res_);
//Refresh on start
on_refreshButton_clicked(); // click a virtual button! :D
}
int main(int argc, char** argv)
{
ROS_INFO("Starting %s ...", NODE_NAME.c_str());
init(argc, argv, NODE_NAME);
NodeHandle n;
Subscriber dataSub = n.subscribe(SUBSCRIBE_TOPIC, MSG_QUEUE_SIZE, irCallBack);
dataPub = n.advertise<geometry_msgs::Twist>(PUBLISH_TOPIC, MSG_QUEUE_SIZE);
if(argc != 4){
std::cout << "Usage: Nodename <FieldView> <MaxDistance> <maxSteering>" << std::endl;
return -1;
}
FIELD = atoi(argv[1]);
MAX_DISTANCE = atoi(argv[2]);
MAX_STEERING = atoi(argv[3]);
while (ok())
spin();
ROS_INFO("Shutting down %s!", NODE_NAME.c_str());
delete lidar_ptr;
}
void
Dbtux::nodePushUpScans(NodeHandle& node, unsigned pos)
{
const unsigned occup = node.getOccup();
ScanOpPtr scanPtr;
scanPtr.i = node.getNodeScan();
do {
jam();
c_scanOpPool.getPtr(scanPtr);
TreePos& scanPos = scanPtr.p->m_scanPos;
ndbrequire(scanPos.m_loc == node.m_loc && scanPos.m_pos < occup);
if (scanPos.m_pos >= pos) {
jam();
#ifdef VM_TRACE
if (debugFlags & DebugScan) {
debugOut << "Fix scan " << scanPtr.i << " " << *scanPtr.p << endl;
debugOut << "At pushUp pos=" << pos << " " << node << endl;
}
#endif
scanPos.m_pos++;
}
scanPtr.i = scanPtr.p->m_nodeScan;
} while (scanPtr.i != RNIL);
}
/*
* Set handle to point to new node. Uses a pre-allocated node.
*/
void
Dbtux::insertNode(NodeHandle& node)
{
Frag& frag = node.m_frag;
// use up pre-allocated node
selectNode(node, frag.m_freeLoc);
frag.m_freeLoc = NullTupLoc;
new (node.m_node) TreeNode();
#ifdef VM_TRACE
TreeHead& tree = frag.m_tree;
memset(node.getPref(), DataFillByte, tree.m_prefSize << 2);
TreeEnt* entList = tree.getEntList(node.m_node);
memset(entList, NodeFillByte, tree.m_maxOccup * (TreeEntSize << 2));
#endif
}
/*
* Check if a scan is linked to this node. Only for ndbrequire.
*/
bool
Dbtux::islinkScan(NodeHandle& node, ScanOpPtr scanPtr)
{
ScanOpPtr currPtr;
currPtr.i = node.getNodeScan();
while (currPtr.i != RNIL) {
jam();
c_scanOpPool.getPtr(currPtr);
if (currPtr.i == scanPtr.i) {
jam();
return true;
}
currPtr.i = currPtr.p->m_nodeScan;
}
return false;
}
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);
}
/*
* Delete existing node. Make it the pre-allocated free node if there
* is none. Otherwise return it to fragment's free list.
*/
void
Dbtux::deleteNode(NodeHandle& node)
{
Frag& frag = node.m_frag;
ndbrequire(node.getOccup() == 0);
if (frag.m_freeLoc == NullTupLoc)
{
jam();
frag.m_freeLoc = node.m_loc;
// invalidate the handle
node.m_loc = NullTupLoc;
node.m_node = 0;
}
else
{
jam();
freeNode(node);
}
}
void getAllPointsRecursiveAsync(MegaTree &tree, NodeHandle& node, double resolution,
std::vector<double> &results, std::vector<double> &colors, megatree::List<NodeHandle*>& nodes_to_load)
{
//printf("Get all points recursive for node %s with frame offset %f %f %f\n",
// node.getId().toString().c_str(), frame_offset[0], frame_offset[1], frame_offset[2]);
assert(!node.isEmpty());
// we hit a leaf
if (node.isLeaf() || node.getNodeGeometry().getSize() <= resolution)
{
double point[3];
node.getPoint(point);
results.push_back(point[0]);
results.push_back(point[1]);
results.push_back(point[2]);
double color[3];
node.getColor(color);
colors.push_back(color[0]);
colors.push_back(color[1]);
colors.push_back(color[2]);
}
// need to descend further
else
{
for(int i = 0; i < 8; ++i)
{
if(node.hasChild(i))
{
megatree::NodeHandle* child = new megatree::NodeHandle;
tree.getChildNode(node, i, *child);
//if the child isn't valid, we'll push it onto our nodes to load list
if(!child->isValid())
{
nodes_to_load.push_back(child);
process_queue_size++;
}
else
{
getAllPointsRecursiveAsync(tree, *child, resolution, results, colors, nodes_to_load);
tree.releaseNode(*child);
delete child;
}
}
}
}
}
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);
}
}
int main(int argc, char** argv) {
init(argc, argv, "JointUpdateNode");
NodeHandle nh;
// get the filenames
nh.param(string("joint_offsets_filename_suffix"), jointOffsetsFilename,
string("calibration_joint_offsets.xacro"));
nh.param(string("camera_transform_filename_suffix"),
cameraTransformFilename,
string("calibration_camera_transform.xacro"));
nh.param(string("urdf_filename_suffix"), urdfFilename,
string("robot_model_calibrated.xml"));
nh.param(string("marker_transforms_filename_suffix"),
markerTransformsFilename,
string("calibration_marker_transformations.xacro"));
nh.param(string("camera_intrnsics_filename"), cameraIntrnsicsFilename,
string("nao_bottom_640x480.yaml"));
ModelLoader modelLoader;
modelLoader.initializeFromRos();
urdf::Model model;
modelLoader.getUrdfModel(model);
// get the robot name and prepend to the filenames
string filePrefix = model.getName();
jointOffsetsFilename = filePrefix + "_" + jointOffsetsFilename;
cameraTransformFilename = filePrefix + "_" + cameraTransformFilename;
urdfFilename = filePrefix + "_" + urdfFilename;
markerTransformsFilename = filePrefix + "_" + markerTransformsFilename;
Subscriber sub = nh.subscribe("/onlineCalibration/calibration_result",
1, resultCb);
spin();
return 0;
}
int main(int argc, char* argv[]) {
gengetopt_args_info args;
cmdline_parser(argc,argv,&args);
float coeffs[6];
coeffs[0] = args.a1_arg;
coeffs[1] = args.a2_arg;
coeffs[2] = args.a3_arg;
coeffs[3] = args.a4_arg;
coeffs[4] = args.a5_arg;
coeffs[5] = args.a6_arg;
sigmaHit = args.sigma_arg;
angleMin = args.angleMin_arg;
angleMax = args.angleMax_arg;
angleMin *= (M_PI/180.0);
angleMax *= (M_PI/180.0);
angleIncrement = (angleMax-angleMin)/args.numBeams_arg;
numBeams = args.numBeams_arg;
float trueDistances[numBeams];
float noisyDistances[numBeams];
float commandReal[2];
init(argc, argv, "Odom");
nav_msgs::Odometry msg;
sensor_msgs::LaserScan msg2;
NodeHandle n;
Publisher pub = n.advertise<nav_msgs::Odometry>("odom", 1);
Publisher las = n.advertise<sensor_msgs::LaserScan>("/scan", 1);
Subscriber sub = n.subscribe("navi", 1000, cmmdUpdate); // update the command velocities
Subscriber rst = n.subscribe("/mobile_base/commands/reset_odometry", 1000, reset);
ros::Duration(1.3).sleep();
ros::Rate loop_rate(10);
while (ros::ok()) {
sampleMotionModel(command, commandReal, pose, coeffs, 0.1);
yawToQuaternion(pose[2], quaternion);
msg.pose.pose.position.x = pose[0];
msg.pose.pose.position.y = pose[1];
msg.pose.pose.orientation.w = quaternion[0];
msg.pose.pose.orientation.x = quaternion[1];
msg.pose.pose.orientation.y = quaternion[2];
msg.pose.pose.orientation.z = quaternion[3];
msg.twist.twist.linear.x = commandReal[0];
msg.twist.twist.angular.z = commandReal[1];
// Laser scanning section
calcTrueDistance(trueDistances, numBeams, angleIncrement);
calcNoisyDistance(noisyDistances, trueDistances, sigmaHit, numBeams);
msg2.angle_max = angleMax;
msg2.angle_min = angleMin;
msg2.time_increment = 0;
msg2.angle_increment = angleIncrement;
msg2.scan_time = 0.1;
msg2.range_min = 0;
msg2.range_max = maxRange;
msg2.ranges.resize(numBeams);
for (int i = 0; i < numBeams; i++) {
msg2.ranges[i] = noisyDistances[i];
}
ros::spinOnce();
pub.publish(msg);
las.publish(msg2);
loop_rate.sleep();
}
return 0;
}
int main(int argc, char **argv)
{
srand(time(0));
ros::init(argc ,argv, "ROS_Publisher");
NodeHandle node;
image_transport::ImageTransport it(node);
std::cout << "Starting image load" << endl;
loadImages();
cout << "Done Loading Images" << endl;
getchar();
mcl_data_subscriber = node.subscribe(mcl_data_publisher_name, 4, MyDataCallback);
time_t temptime = time(0);
std::cout << "Waiting for Handshake from Program .." << std::endl;
while(!handshake_recieved && (time(0) - temptime) < 20)
{
ros::spinOnce();
}
if(handshake_recieved)
std::cout << "Handshake recieved" << std::endl;
else
{
std::cout << "No handshake recieved";
return -1;
}
movement_publisher = node.advertise<std_msgs::String>("ROBOT_MOVEMENT_PUBLISHER", 4);
data_publisher = it.advertise(publish_image_data_under, 4, true);
char key = 'k';
// namedWindow("Robot Image");
// namedWindow("Top Match");
while(ros::ok() && key != 'q')
{
ros::spinOnce();
int i = rand()%image_names.size();
cv_bridge::CvImage out_msg;
ros::Time scan_time = ros::Time::now();
out_msg.header.stamp = scan_time;
out_msg.header.frame_id = "robot_image";
out_msg.encoding = sensor_msgs::image_encodings::BGR8;
out_msg.image = image_list[current_image];
if(!out_msg.image.empty())
data_publisher.publish(out_msg.toImageMsg());
ros::spinOnce();
std::cout << "hererer" << std::endl;
// imshow("Robot Image", image_list[current_image]);
// imshow("Top Match", BestGuessImage);
// key = cv::waitKsey(2);
if(key == ' ')
current_image++;
if(current_image == image_list.size())
current_image = 0;
//ros::Duration(0.1).sleep();
}
std_msgs::String msg;
stringstream ss;
ss << killflag << "_";
msg.data = ss.str();
movement_publisher.publish(msg);
ros::spinOnce();
ros::spinOnce();
ros::Duration(0.1).sleep();
ros::spinOnce();
ros::shutdown();
}
FeatherNodesFixture::~FeatherNodesFixture() {
FeatherNodeFactory::instance().reset();
}
void FeatherNodesFixture::clearAuxNodes() {
FeatherNodeFactory::instance().clearAuxNodes();
}
void FeatherNodesFixture::setContextForAuxNodes() {
FeatherNodeFactory::instance().setContextForAuxNodes(globalContext_);
}
TEST_CASE_METHOD(FeatherNodesFixture, "Testing Feather expressions generation") {
rc::prop("Test expected type", [=](Nest::TypeWithStorage expectedType) {
clearAuxNodes();
NodeHandle node = *FeatherNodeFactory::instance().arbExp(expectedType);
setContextForAuxNodes();
node.setContext(globalContext_);
auto t = node.computeType();
REQUIRE(t);
if (expectedType.kind() != Feather_getFunctionTypeKind())
REQUIRE(sameTypeIgnoreMode(t, expectedType));
});
}
TEST_CASE_METHOD(FeatherNodesFixture, "Testing Feather::Nop node") {
SECTION("Has type Void") {
clearAuxNodes();
auto nop = Nop::create(createLocation());
setContextForAuxNodes();
nop.setContext(globalContext_);
NodeHandle FileEntry::create_hidden_node( NodeHandle n ) {
NodeHandle r = StringEntry::create_hidden_node(n);
r->add_attribute( default_extension );
return r;
}
void getParameters(NodeHandle &nh)
{
////////////////////////////////////////////////////////////
// Get robot description
////////////////////////////////////////////////////////////
string description;
if(!nh.getParam("/robot_description", description))
{
ROS_ERROR("Parameter not set: /robot_description");
exit(-1);
}
////////////////////////////////////////////////////////////
// Create robot model
////////////////////////////////////////////////////////////
urdf::Model model;
if(!model.initString(description))
{
ROS_ERROR("Could not initialize robot model");
exit(-1);
}
////////////////////////////////////////////////////////////
// Get tip link name and verify it exists in the model
////////////////////////////////////////////////////////////
string tip_link_name;
if(!nh.getParam("tip_link_name", tip_link_name))
{
ROS_ERROR("Parameter not set: %s/tip_link_name", nh.getNamespace().c_str());
exit(-1);
}
if(model.links_.find(tip_link_name) == model.links_.end())
{
ROS_ERROR("The link specified in %s/tip_link_name does not exist", nh.getNamespace().c_str());
exit(-1);
}
////////////////////////////////////////////////////////////
// Get revolute joints
////////////////////////////////////////////////////////////
vector<boost::shared_ptr<urdf::Joint> > model_joints;
for(boost::shared_ptr<const urdf::Link> current_link = model.getLink(tip_link_name); current_link->parent_joint != NULL; current_link = current_link->getParent())
{
if(current_link->parent_joint->type == urdf::Joint::REVOLUTE)
{
model_joints.insert(model_joints.begin(), current_link->parent_joint);
}
}
if(model_joints.size() != NUM_SERVOS)
{
ROS_ERROR("The robot model must have %d revolute joints, found %d", NUM_SERVOS, (int)model_joints.size());
exit(-1);
}
////////////////////////////////////////////////////////////
// Configure servo limits
////////////////////////////////////////////////////////////
for(int joint_i = 0; joint_i < NUM_SERVOS; joint_i++)
{
float lowerLimit = model_joints[joint_i]->limits->lower;
float upperLimit = model_joints[joint_i]->limits->upper;
RSSerialController::setServoLimits(joint_i, lowerLimit, upperLimit);
}
////////////////////////////////////////////////////////////
// Store joint names
////////////////////////////////////////////////////////////
for(int joint_i = 0; joint_i < NUM_SERVOS; joint_i++)
{
joint_names.push_back(model_joints[joint_i]->name);
}
////////////////////////////////////////////////////////////
// Create KDL tree
////////////////////////////////////////////////////////////
if(!kdl_parser::treeFromString(description, tree))
{
ROS_ERROR("Failed to construct kdl tree");
exit(-1);
}
if(!tree.getChain(model.getRoot()->name, tip_link_name, chain))
{
ROS_ERROR("Failed to make chain from tree");
exit(-1);
}
}
int main(int argc, char** argv)
{
//initialize ros
init(argc, argv, ROS_NODE_NAME);
NodeHandle n;
Rate loop_rate(ROS_LOOP_RATE);
//initialize serial communication
SerialCommunication link;
for (int i = 0; ; i++)
{
stringstream ss;
ss << i;
if (link.connect(BAUD_RATE,(PORT_NAME + ss.str())))
{
cout << "connected on port " << i << endl;
break;
} else if (link.connect(BAUD_RATE,(UNO_PORT_NAME + ss.str())))
{
cout << "connected on port " << i << endl;
break;
} else if (link.connect(BAUD_RATE,(BLUETOOTH_PORT_NAME + ss.str()))) {
cout << "connected on bluetooth port " << i << endl;
break;
} else if (i > 15) {
cout << "unable to find a device" << endl;
return 0;
}
}
usleep(1*SECOND);
//subscribers and publishers
Subscriber car_command = n.subscribe(CAR_COMMAND_TOPIC, 1, car_command_callback);
Subscriber turret_command = n.subscribe(TURRET_COMMAND_TOPIC, 1, turret_command_callback);
Subscriber eStop_topic = n.subscribe(ESTOP_TOPIC, 1, eStop_callback);
Subscriber gps_timeread = n.subscribe(GPS_TIMEREAD_TOPIC, 1, gps_timeread_callback); // gps
Publisher gps_position = n.advertise<sensor_msgs::NavSatFix>(GPS_POSITION_TOPIC, 1); // gps
Publisher gps_timestamp = n.advertise<sensor_msgs::TimeReference>(GPS_TIMESTAMP_TOPIC, 1); // gps
Publisher robot_state = n.advertise<sb_msgs::RobotState>(ROBOT_STATE_TOPIC,1);
/*
//handshake with arduino and initialize robot
for(;;)
{
link.writeData(INIT_STRING,2);
usleep(20000);
string handshake = link.readData();
if(handshake.length() == 2)
{
if(handshake[0] == INIT_STRING[0] && handshake[1] == INIT_STRING[1])
{
break;
}
}
}
*/
sb_msgs::IMU imu;
sensor_msgs::NavSatFix fix; // gps position message
sensor_msgs::TimeReference time; // gps timestamp message
sb_msgs::RobotState state;
ROS_INFO("arduino_driver ready");
servo.throttle = 90;
servo.steering = 90;
servo.pan = 90;
servo.tilt = 90;
//while ros is good
ros::Time begin = ros::Time::now();
int counter = 0;
link.clearBuffer();
while(ok())
{
if (int((ros::Time::now() - begin).toSec()) >= 1) {
cout << counter << endl;
counter = 0;
begin = ros::Time::now();
}
counter++;
//write to arduino
stringstream ss;
if (eStop)
{
cout << "eStop on" << endl;
ss << (char)IDENTIFIER_BYTE << (char)90 << (char)90 << (char)255 << (char)255;
} else {
//use carCommand and turretCommand
ss << (char)IDENTIFIER_BYTE << (char)servo.throttle << (char)servo.steering << (char)servo.pan << (char)servo.tilt;
}
link.writeData(ss.str(), 5);
//delay for sync
usleep(20000);
//read from arduino
//processData(link.readData(), state);
//populate GUI data
//TODO
//publish data
robot_state.publish(state);
gps_position.publish(fix); // gps
gps_timestamp.publish(time); // gps
//clear buffer (MAY NOT WORK)
link.clearBuffer();
//log and loop
ROS_INFO("%i,%i,%i,%i",servo.throttle, servo.steering, servo.pan, servo.tilt);
spinOnce();
loop_rate.sleep();
}
//killing driver
ROS_INFO("shutting down arduino_driver");
string s = "ED" ;
link.writeData(s, 2);
return 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;
}
void TreeFastCache::addPointRecursive(const double pt[3], const double color[3], double point_accuracy)
{
NodeHandle* nh = top().nh;
assert(nh);
double node_cell_size = nh->getNodeGeometry().getSize();
// adds point to empty node
if (nh->isEmpty() && point_accuracy >= node_cell_size / BITS_D)
{
nh->setPoint(pt, color);
return;
}
// reached maximum resultion of the tree
if (node_cell_size < tree.getMinCellSize())
{
nh->addPoint(pt, color);
return;
}
// this node is a leaf, and we need to copy the original point one level down
if (!nh->isEmpty() && nh->isLeaf())
{
// Determines which child the point should be added to.
uint8_t original_child = nh->getChildForNodePoint();
NodeHandle new_leaf;
tree.createChildNode(*nh, original_child, new_leaf);
nh->getNode()->copyToChildNode(original_child, new_leaf.getNode());
tree.releaseNode(new_leaf);
}
// Gets the child node to recurse on.
uint8_t new_child = nh->getNodeGeometry().whichChild(pt);
NodeHandle* new_child_nh = NULL;
if (nh->hasChild(new_child)) {
new_child_nh = tree.getChildNode(*nh, new_child);
new_child_nh->waitUntilLoaded();
}
else {
new_child_nh = tree.createChildNode(*nh, new_child);
}
// recursion to add point to child
push(NodeCache(new_child_nh));
addPointRecursive(pt, color, point_accuracy);
}
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;
}
void addPointRecursive(MegaTree& tree, NodeHandle& node,
const double* pt, const double* color,
double point_accuracy)
{
//printf("Adding point %f %f %f to node %s\n", pt[0], pt[1], pt[2], node.toString().c_str());
double node_cell_size = node.getNodeGeometry().getSize();
// adds point to empty node
if (node.isEmpty() && point_accuracy >= node_cell_size / BITS_D)
{
node.setPoint(pt, color);
return;
}
// reached maximum resultion of the tree
if (node_cell_size < tree.getMinCellSize())
{
node.addPoint(pt, color);
return;
}
// this node is a leaf, and we need to copy the original point one level down
if (!node.isEmpty() && node.isLeaf())
{
// Determines which child the point should be added to.
uint8_t original_child = node.getChildForNodePoint();
//create_leaf_timer.start();
NodeHandle new_leaf;
tree.createChildNode(node, original_child, new_leaf);
//create_leaf_timer.pause();
node.getNode()->copyToChildNode(original_child, new_leaf.getNode());
tree.releaseNode(new_leaf);
}
// Gets the child node to recurse on.
uint8_t new_child = node.getNodeGeometry().whichChild(pt);
NodeHandle new_child_node;
if (node.hasChild(new_child)) {
//getnode_timer.start();
tree.getChildNode(node, new_child, new_child_node);
new_child_node.waitUntilLoaded();
//getnode_timer.pause();
}
else {
//create_node_timer.start();
tree.createChildNode(node, new_child, new_child_node);
//create_node_timer.pause();
}
// recursion to add point to child
addPointRecursive(tree, new_child_node, pt, color, point_accuracy);
tree.releaseNode(new_child_node);
// Re-computes the summary point.
MegaTree::ChildIterator it(tree, node, new_child_node.getNodeFile());
node.copyFromChildNodes(it.getAllChildren());
}