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";
}
