virtual void onInit() { std::string name = this->getName(); std::string parent_namespace = ros::names::parentNamespace(name); ros::NodeHandle nh = this->getNodeHandle(); ros::NodeHandle pnh = this->getPrivateNodeHandle(); public_pub = nh.advertise<std_msgs::String>("public", 1000); private_pub = pnh.advertise<std_msgs::String>("private", 1000); NODELET_INFO_STREAM("Dude : initialising nodelet..." << name); NODELET_INFO_STREAM("Dude : parent namespace -> " << parent_namespace); NODELET_INFO_STREAM("Dude : public topic -> " << public_pub.getTopic()); NODELET_INFO_STREAM("Dude : private topic -> " << private_pub.getTopic()); }
void Dm32ToDmNodelet::onInit() { nh_ = getNodeHandle(); private_nh_ = getPrivateNodeHandle(); nh_.param("sensor/name", sensor_name_, std::string("realsenseR200")); image_transport::ImageTransport it(nh_); image_pub_ = it.advertise("dm32_to_dm/output", 1); sub_ = it.subscribe("dm32_to_dm/input", 1, &Dm32ToDmNodelet::cb, this); scale_ = 0.001; if(sensor_name_ == "realsenseF200") { sensor_depth_max_ = 1200; } else if(sensor_name_ == "realsenseR200") { private_nh_.param("sensor_depth_max", sensor_depth_max_, int(4000)); } else if(sensor_name_ == "picoflexx") { nh_.param("sensor/depth/max", sensor_depth_max_, int(5000)); } else { NODELET_ERROR("Wrong sensor name!"); return; } NODELET_INFO_STREAM("Initialized dm32 to dm nodelet for sensor " << sensor_name_ << ", with sensor_depth_max=" << sensor_depth_max_); }
void ControlMode::controlModeCmdCallback(const control_mode_cmdConstPtr& msg) { NODELET_INFO_STREAM("Heard command: " << msg->cmd); if (msg->cmd == "mode idle") { state = ControlModeTypes::IDLE; info = ""; } else if (msg->cmd == "mode standby") { state = ControlModeTypes::STANDBY; info = ""; } else if (msg->cmd == "mode active") { if (state == ControlModeTypes::STANDBY) { state = ControlModeTypes::ACTIVE; info = ""; } else { NODELET_ERROR("Invalid transition"); } } else { NODELET_ERROR_STREAM("Command unknown: " << msg->cmd); } }
virtual void onInit() { ros::NodeHandle nh = this->getPrivateNodeHandle(); // resolve node(let) name std::string name = nh.getUnresolvedNamespace(); int pos = name.find_last_of('/'); name = name.substr(pos + 1); NODELET_INFO_STREAM("Initialising nodelet... [" << name << "]"); controller_.reset(new SafetyController(nh, name)); if (controller_->init()) { NODELET_INFO_STREAM("Kobuki initialised. Spinning up update thread ... [" << name << "]"); update_thread_.start(&SafetyControllerNodelet::update, *this); NODELET_INFO_STREAM("Nodelet initialised. [" << name << "]"); } else { NODELET_ERROR_STREAM("Couldn't initialise nodelet! Please restart. [" << name << "]"); } }
void ImageNodelet::onInit() { ros::NodeHandle nh = getNodeHandle(); ros::NodeHandle local_nh = getPrivateNodeHandle(); // Command line argument parsing const std::vector<std::string>& argv = getMyArgv(); // First positional argument is the transport type std::string transport; local_nh.param("image_transport", transport, std::string("raw")); for (int i = 0; i < (int)argv.size(); ++i) { if (argv[i][0] != '-') { transport = argv[i]; break; } } NODELET_INFO_STREAM("Using transport \"" << transport << "\""); // Internal option, should be used only by the image_view node bool shutdown_on_close = std::find(argv.begin(), argv.end(), "--shutdown-on-close") != argv.end(); // Default window name is the resolved topic name std::string topic = nh.resolveName("image"); local_nh.param("window_name", window_name_, topic); bool autosize; local_nh.param("autosize", autosize, false); std::string format_string; local_nh.param("filename_format", format_string, std::string("frame%04i.jpg")); filename_format_.parse(format_string); cv::namedWindow(window_name_, autosize ? cv::WND_PROP_AUTOSIZE : 0); cv::setMouseCallback(window_name_, &ImageNodelet::mouseCb, this); #ifdef HAVE_GTK // Register appropriate handler for when user closes the display window GtkWidget *widget = GTK_WIDGET( cvGetWindowHandle(window_name_.c_str()) ); if (shutdown_on_close) g_signal_connect(widget, "destroy", G_CALLBACK(destroyNode), NULL); else g_signal_connect(widget, "destroy", G_CALLBACK(destroyNodelet), &sub_); #endif // Start the OpenCV window thread so we don't have to waitKey() somewhere startWindowThread(); image_transport::ImageTransport it(nh); image_transport::TransportHints hints(transport, ros::TransportHints(), getPrivateNodeHandle()); sub_ = it.subscribe(topic, 1, &ImageNodelet::imageCb, this, hints); }
void ServoInterface::loadServoCommandPriorities() { ros::NodeHandle nhPvt = getPrivateNodeHandle(); XmlRpc::XmlRpcValue v; nhPvt.param("servoCommandProirities", v, v); std::map<std::string, XmlRpc::XmlRpcValue>::iterator mapIt; for(mapIt = v.begin(); mapIt != v.end(); mapIt++) { if(mapIt->second.getType() == XmlRpc::XmlRpcValue::TypeInt) { //add entry in priority queue and command map priorityEntry toAdd; toAdd.id = mapIt->first; toAdd.priority = static_cast<int>(mapIt->second); m_servoCommandPriorities.push_back(toAdd); m_servoCommandMsgs[mapIt->first] = autorally_msgs::servoMSG(); } else { NODELET_ERROR("ServoInterface: XmlRpc servo command priorities formatted incorrectly"); } } std::sort(m_servoCommandPriorities.begin(), m_servoCommandPriorities.end(), priorityComparator()); std::vector<priorityEntry>::const_iterator vecIt; for(vecIt = m_servoCommandPriorities.begin(); vecIt != m_servoCommandPriorities.end(); vecIt++) { NODELET_INFO_STREAM("ServoInterface: ServoCommand ID:Priorities:" << vecIt->id << ":" << vecIt->priority); } NODELET_INFO_STREAM("ServoInterface: Loaded " << m_servoCommandPriorities.size() << " servo commanders"); }
void ServoInterface::onInit() { ros::NodeHandle nh = getNodeHandle(); ros::NodeHandle nhPvt = getPrivateNodeHandle(); std::string port; if(!nh.getParam(getName()+"/port", port)) { NODELET_ERROR("ServoInterface: could not get servo interface port"); } loadServoParams(); loadServoCommandPriorities(); m_maestro.init(nh, getName(), port); m_ss.init(nh); double servoCommandRate = 0.0; if(!nhPvt.getParam("servoCommandRate", servoCommandRate) || !nhPvt.getParam("throttleBrakeCoupled", m_brakeSetup.coupledWithThrottle) || !nhPvt.getParam("servoCommandMaxAge", m_servoCommandMaxAge)) { NODELET_ERROR_STREAM(getName() << " could not get all startup params"); } for (auto& mapIt : m_servoCommandMsgs) { ros::Subscriber sub = nh.subscribe(mapIt.first+"/servoCommand", 2, &ServoInterface::servoMSGCallback, this); m_servoSub[mapIt.first] = sub; NODELET_INFO_STREAM("ServoInterface: subscribed to srv cmd:" << mapIt.first+"/servoCommand"); } m_speedCommandSub = nh.subscribe("vehicleSpeedCommand", 2, &ServoInterface::speedCallback, this); //m_servoStatusTimer = nh.createTimer(ros::Rate(servoStatusPubRate), // &ServoInterface::servoStatusTimerCallback, this); m_throttleTimer = nh.createTimer(ros::Rate(servoCommandRate), &ServoInterface::setServos, this); m_servoMSGPub = nh.advertise<autorally_msgs::servoMSG> ("servoStatus", 2); //m_servoMSGStatus = autorally_msgs::servoMSGPtr(new autorally_msgs::servoMSG); //m_servoMSGStatus->header.frame_id = "servoController"; }