int main(int argc, char** argv)
{
ros::init(argc, argv, "vision_node");
ros::NodeHandle nh;
// Initialize node parameters from launch file or command line.
// Use a private node handle so that multiple instances of the node can be run simultaneously
// while using different parameters.
ros::NodeHandle private_node_handle_("~");
// Declare variables that can be modified by launch file or command line.
int rate;
private_node_handle_.param("rate", rate, 100);
StereoRos<ConventionalStereo> stereo_ros(nh, private_node_handle_);
// Tell ROS how fast to run this node.
ros::Rate r(rate);
// Main loop.
while (nh.ok())
{
ros::spinOnce();
r.sleep();
}
return 0;
} // end
TerarangerOne::TerarangerOne()
{
// Get paramters
ros::NodeHandle private_node_handle_("~");
private_node_handle_.param("portname", portname_, std::string("/dev/ttyUSB0"));
// Publishers
range_publisher_ = nh_.advertise<sensor_msgs::Range>("terarangerone", 1);
// Create serial port
serial_port_ = new SerialPort();
// Set callback function for the serial ports
serial_data_callback_function_ = boost::bind(&TerarangerOne::serialDataCallback, this, _1);
serial_port_->setSerialCallbackFunction(&serial_data_callback_function_);
// Connect serial port
if (!serial_port_->connect(portname_))
{
ros::shutdown();
return;
}
// Output loaded parameters to console for double checking
ROS_INFO("[%s] is up and running with the following parameters:", ros::this_node::getName().c_str());
ROS_INFO("[%s] portname: %s", ros::this_node::getName().c_str(), portname_.c_str());
// Set operation Mode
setMode(BINARY_MODE);
// Dynamic reconfigure
dyn_param_server_callback_function_ = boost::bind(&TerarangerOne::dynParamCallback, this, _1, _2);
dyn_param_server_.setCallback(dyn_param_server_callback_function_);
}
void parseCommandLine()
{
ros::NodeHandle private_node_handle_("~");
private_node_handle_.getParam("res", resolution);
private_node_handle_.getParam("noise", noise_filter);
private_node_handle_.getParam("mean_k", sor_mean_k);
private_node_handle_.getParam("std_dev_mul", sor_stddev_mul_th);
private_node_handle_.getParam("rad", ror_rad);
private_node_handle_.getParam("min_neighbors", ror_min_heighbors);
}
int main(int argc, char **argv)
{
// Set up ROS.
ros::init(argc, argv, "mvsim");
ros::NodeHandle n;
// Create a "Node" object.
MVSimNode node(n);
// Declare variables that can be modified by launch file or command line.
int rate;
std::string world_file;
// Initialize node parameters from launch file or command line.
// Use a private node handle so that multiple instances of the node can be run simultaneously
// while using different parameters.
ros::NodeHandle private_node_handle_("~");
private_node_handle_.param("simul_rate", rate, 100);
private_node_handle_.param("world_file", world_file, std::string(""));
// Init world model:
if (!world_file.empty())
node.loadWorldModel(world_file);
// Set up a dynamic reconfigure server.
// Do this before parameter server, else some of the parameter server
// values can be overwritten.
dynamic_reconfigure::Server<mvsim::mvsimNodeConfig> dr_srv;
dynamic_reconfigure::Server<mvsim::mvsimNodeConfig>::CallbackType cb;
cb = boost::bind(&MVSimNode::configCallback, &node, _1, _2);
dr_srv.setCallback(cb);
// Create a publisher and name the topic.
//ros::Publisher pub_message = n.advertise<node_example::NodeExampleData>("example", 10);
// Name the topic, message queue, callback function with class name, and object containing callback function.
//ros::Subscriber sub_message = n.subscribe("example", 1000, &NodeExample::messageCallback, node_example);
// Tell ROS how fast to run this node.
ros::Rate r(rate);
// Main loop.
while (n.ok())
{
node.spin();
ros::spinOnce();
r.sleep();
}
return 0;
} // end main()
/**
* @brief Create a ROS node that publishes fingerprints.
*
* This is the main function to set up the ROS node.
**/
int main(int argc, char **argv)
{
/* Set up ROS, get handle, set desired rate. */
ros::init(argc, argv, "fingerprint");
ros::NodeHandle node;
ros::Rate rate(1);
/* Get parameters from command line. */
ros::NodeHandle private_node_handle_("~");
std::string topic_name;
private_node_handle_.param<std::string>("topic", topic_name, "wifi_fp");
std::string interface;
private_node_handle_.param<std::string>("interface", interface, "wlan0");
/* Channels are hard-coded for now. */
std::vector<int> channels;
channels.push_back(1);
channels.push_back(6);
channels.push_back(11);
/* Create topic, scanning object, and start fingerprinting. */
ros::Publisher pub_fingerprint = node.advertise<wifi_scan::Fingerprint>(
topic_name, 10);
WifiScan wifiscan(channels, interface);
while (node.ok())
{
try
{
wifiscan.createFingerprint(&pub_fingerprint);
}
catch (int exception)
{
switch (exception)
{
case WIFISCAN_ERROR_OPENING_IOCTL_SOCKET:
ROS_ERROR_STREAM("Error in WifiScan::createFingerprint:\n"
<< " Error opening ioctl socket.");
break;
case WIFISCAN_ERROR_IN_IW_SCAN:
ROS_ERROR_STREAM(
"Error in WifiScan::createFingerprint:\n"
<< " Error in iw_scan(). Might be wrong interface.");
break;
default:
ROS_ERROR_STREAM("Error in WifiScan::createFingerprint:\n"
<< " Unknown error.");
}
}
ros::spinOnce();
rate.sleep();
}
}
active_survey::active_survey(int argc, char **argv):
nh_("active_survey")
{
ros::NodeHandle private_node_handle_("~");
active_survey_param::GetParams(private_node_handle_);
mav_ = std::make_shared<mav>(*environment_model::instance(), Vector3f{-0.5f*static_cast<float>(active_survey_param::area_width),
-0.5f*static_cast<float>(active_survey_param::area_height),
10.0f});
if(active_survey_param::logging)
{
setup_log_file();
}
active_survey_log("this is a test for active_survey logging system.");
}
ColorHistogramDescriptorNode() {
ros::NodeHandle nh;
dynamic_reconfigure::Server<saliency_detector::color_histogram_descriptor_paramsConfig>::CallbackType cb;
cb = boost::bind(&ColorHistogramDescriptorNode::configCallback, this, _1, _2);
dr_srv.setCallback(cb);
ros::NodeHandle private_node_handle_("~");
sub_patch_array =
nh.subscribe("projected_patch_array", 1, &ColorHistogramDescriptorNode::patchArrayCallback, this);
pub_named_points =
nh.advertise<samplereturn_msgs::NamedPointArray>("named_point", 1);
pub_debug_image =
nh.advertise<sensor_msgs::Image>("color_debug_image", 1);
}
KalmanDetectionFilter::KalmanDetectionFilter()
{
dynamic_reconfigure::Server<detection_filter::ManipulatorKalmanFilterConfig>::CallbackType cb;
cb = boost::bind(&KalmanDetectionFilter::configCallback, this, _1, _2);
dr_srv.setCallback(cb);
ros::NodeHandle private_node_handle_("~");
private_node_handle_.param("filter_frame_id", _filter_frame_id, std::string("odom"));
last_negative_measurement_ = ros::Time(0);
updated_ = false;
filter_id_count_ = 0;
sub_detections =
nh.subscribe("point", 3, &KalmanDetectionFilter::detectionCallback, this);
pub_detection =
nh.advertise<samplereturn_msgs::NamedPoint>("filtered_point", 3);
pub_debug_img =
nh.advertise<sensor_msgs::Image>("debug_img", 3);
}
int main(int argc, char** argv) {
// Set up ROS.
ros::init(argc, argv, "slice_client");
ros::NodeHandle n;
// Initialize node parameters from launch file or command line.
// Use a private node handle so that multiple instances of the node can be run simultaneously
// while using different parameters.
ros::NodeHandle private_node_handle_("~");
//Set up the clients, one to call for slices, the other to order stims
ros::ServiceClient sliceClient = n.serviceClient<img_slicer::ImageSlicer>("red_px_counts");
ros::ServiceClient stimClient = n.serviceClient<zanni::Stim>("deliver_stim");
//Rate to loop in Hz
ros::Rate r(10);
//Run in a loop forever, making requests
img_slicer::ImageSlicer srv;
srv.request.slices = 5;
//Set up plotter for visualization
XPlotter* plot = setupDisplay();
//Run in a loop making the calls
while (n.ok()) {
if (sliceClient.call(srv)) {
redrawDisplay(plot, srv.response.pixelCount, MODE);
uint64_t leftCount = 0;
uint64_t rightCount = 0;
for (int ii = 0; ii < 3; ii++) {
leftCount += srv.response.pixelCount[ii];
rightCount += srv.response.pixelCount[4 - ii];
}
ROS_INFO("R: %lu L: %lu\n", leftCount, rightCount);
//Don't bother stimulating at all unless difference is big
if (abs(leftCount - rightCount) > 500) {
//Fill in the service request with the data from the header file.
zanni::Stim stimSrv;
int elements = sizeof(training_signal) / sizeof(training_signal[0]);
for (int ii = 0; ii < elements; ii++) {
stimSrv.request.signal.push_back(training_signal[ii] * 10000);
}
//Pick a channel
if (leftCount > rightCount) {
stimSrv.request.channel = 0; //Arbitrary decision that 0 = left
} else {
stimSrv.request.channel = 1;
}
//Make the call
if (stimClient.call(stimSrv)) {
if (stimSrv.response.done) {
ROS_INFO("Stimulation sent");
} else {
ROS_WARN("Problem on stimulation end");
}
} else {
ROS_ERROR("Failed to call stimulation");
return 1;
}
}
} else {
ROS_ERROR("Call failed to get pixel counts");
}
r.sleep();
}
return 0;
}
int main(int argc, char* argv[])
{
ros::init(argc, argv, "realsense_camera_node");
ros::NodeHandle n;
image_transport::ImageTransport image_transport(n);
ros::NodeHandle private_node_handle_("~");
private_node_handle_.param("realsense_camera_type", realsense_camera_type, std::string("Intel(R) RealSense(TM) 3D Camer"));
private_node_handle_.param("rgb_frame_id", rgb_frame_id, std::string("_rgb_optical_frame"));
private_node_handle_.param("depth_frame_id", depth_frame_id, std::string("_depth_optical_frame"));
private_node_handle_.param("rgb_frame_w", rgb_frame_w, 1280);
private_node_handle_.param("rgb_frame_h", rgb_frame_h, 720);
double depth_unit_d, depth_scale_d;
private_node_handle_.param("depth_unit", depth_unit_d, 31.25);
private_node_handle_.param("depth_scale", depth_scale_d, 0.001);
depth_unit = depth_unit_d;
depth_scale = depth_scale_d;
double depth_fx, depth_fy;
private_node_handle_.param("depth_fx", depth_fx, 463.888885);
private_node_handle_.param("depth_fy", depth_fy, 463.888885);
depth_fxinv = 1.0f / depth_fx;
depth_fyinv = 1.0f / depth_fy;
double depth_cx_d, depth_cy_d;
private_node_handle_.param("depth_cx", depth_cx_d, 320.0);
private_node_handle_.param("depth_cy", depth_cy_d, 240.0);
depth_cx = depth_cx_d;
depth_cy = depth_cy_d;
private_node_handle_.param("depth_uv_enable_min", depth_uv_enable_min, 0);
private_node_handle_.param("depth_uv_enable_max", depth_uv_enable_max, 2047);
private_node_handle_.param("topic_depth_points_id", topic_depth_points_id, std::string("/depth/points"));
private_node_handle_.param("topic_depth_registered_points_id", topic_depth_registered_points_id, std::string("/depth_registered/points"));
private_node_handle_.param("topic_image_rgb_raw_id", topic_image_rgb_raw_id, std::string("/rgb/image_raw"));
private_node_handle_.param("topic_image_depth_raw_id", topic_image_depth_raw_id, std::string("/depth/image_raw"));
private_node_handle_.param("topic_image_infrared_raw_id", topic_image_infrared_raw_id, std::string("/ir/image_raw"));
private_node_handle_.param("debug_depth_unit", debug_depth_unit, false);
std::string rgb_info_url;
private_node_handle_.param("rgb_camera_info_url", rgb_info_url, std::string());
std::string ir_camera_info_url;
private_node_handle_.param("ir_camera_info_url", ir_camera_info_url, std::string());
printf("\n\n===================\n"
"realsense_camera_type = %s\n"
"rgb_frame_id = %s\n"
"depth_frame_id = %s\n"
"depth_unit = %f\n"
"depth_scale = %f\n"
"depth_fxinv = %f\n"
"depth_fyinv = %f\n"
"depth_cx = %f\n"
"depth_cy = %f\n"
"depth_uv_enable_min = %d\n"
"depth_uv_enable_max = %d\n"
"topic_depth_points_id = %s\n"
"topic_depth_registered_points_id = %s\n"
"topic_image_rgb_raw_id = %s\n"
"topic_image_depth_raw_id = %s\n"
"topic_image_infrared_raw_id = %s\n"
"debug_depth_unit = %d\n"
"rgb_camera_info_url = %s\n"
"ir_camera_info_url = %s\n"
"=======================\n\n",
realsense_camera_type.c_str(),
rgb_frame_id.c_str(),
depth_frame_id.c_str(),
depth_unit,
depth_scale,
depth_fxinv,
depth_fyinv,
depth_cx,
depth_cy,
depth_uv_enable_min,
depth_uv_enable_max,
topic_depth_points_id.c_str(),
topic_depth_registered_points_id.c_str(),
topic_image_rgb_raw_id.c_str(),
topic_image_depth_raw_id.c_str(),
topic_image_infrared_raw_id.c_str(),
debug_depth_unit,
rgb_info_url.c_str(),
ir_camera_info_url.c_str()
);
#ifdef V4L2_PIX_FMT_INZI
printf("\ndepthWithIRStream - YEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEES\n");
#else
printf("\ndepthWithIRStream - NOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOO\n");
printf("if you want IR stream, please visit\n"
"http://solsticlipse.com/2015/03/31/intel-real-sense-3d-on-linux-macos.html\n"
"https://github.com/teknotus/depthview/tree/kernelpatchfmt\n");
#endif
//find realsense video device
std::vector<VIDEO_DEVICE> video_lists;
list_devices(realsense_camera_type, video_lists);
if(video_lists.empty())
{
printf("\n\n""can not find Intel(R) RealSense(TM) 3D Camera video device!!!!!!!!! - %s\n\n", realsense_camera_type.c_str());
ROS_ERROR("can not find Intel(R) RealSense(TM) 3D Camera video device!!!!!!!!! - %s", realsense_camera_type.c_str());
ros::shutdown();
return 0;
}
if(1)
{
printf("\n===========================================\n");
printf("Intel(R) RealSense(TM) 3D Camera lists\n");
for(int i=0; i<video_lists.size(); ++i)
{
printf("\nPCI: %s\n", video_lists[i].card_name.c_str());
printf("Serial: %s\n", video_lists[i].serial_number.c_str());
for(int j=0; j<video_lists[i].video_names.size(); ++j)
{
printf("\t%s\n", video_lists[i].video_names[j].c_str());
}
}
printf("===========================================\n\n");
}
//return 0;
if(video_lists[0].video_names.size() < 2)
{
printf("Intel(R) RealSense(TM) 3D Camera video device count error!!!!!!!!!!!\n");
ros::shutdown();
return 0;
}
else
{
useDeviceSerialNum = video_lists[0].serial_number;
printf("use camera %s\n", useDeviceSerialNum.c_str());
}
initDepthToRGBUVMap();
initVideoStream();
strncpy(rgb_stream.videoName, video_lists[0].video_names[0].c_str(), video_lists[0].video_names[0].length());
strncpy(depth_stream.videoName, video_lists[0].video_names[1].c_str(), video_lists[0].video_names[1].length());
printf("video rgb name is %s\n", rgb_stream.videoName);
printf("video depth name is %s\n", depth_stream.videoName);
if(capturer_mmap_init(&rgb_stream))
{
printf("open %s error!!!!!!!!\n", rgb_stream.videoName);
ros::shutdown();
return 0;
}
else
{
printf("video rgb w,h - %d, %d\n", rgb_stream.width, rgb_stream.height);
}
if(capturer_mmap_init(&depth_stream))
{
printf("open %s error!!!!!!!!\n", depth_stream.videoName);
ros::shutdown();
return 0;
}
else
{
printf("video depth w,h - %d, %d\n", depth_stream.width, depth_stream.height);
}
if (!rgb_info_url.empty())
{
std::string camera_name_rgb = "realsense_camera_rgb_" + useDeviceSerialNum;
camera_info_manager::CameraInfoManager rgb_info_manager(n, camera_name_rgb, rgb_info_url);
if (rgb_info_manager.isCalibrated())
{
rgb_camera_info = boost::make_shared<sensor_msgs::CameraInfo>(rgb_info_manager.getCameraInfo());
if (rgb_camera_info->width != rgb_frame_w || rgb_camera_info->height != rgb_frame_h)
{
ROS_WARN("RGB image resolution does not match calibration file");
rgb_camera_info.reset(new sensor_msgs::CameraInfo());
rgb_camera_info->width = rgb_frame_w;
rgb_camera_info->height = rgb_frame_h;
}
}
}
if (!rgb_camera_info)
{
rgb_camera_info.reset(new sensor_msgs::CameraInfo());
rgb_camera_info->width = rgb_frame_w;
rgb_camera_info->height = rgb_frame_h;
}
if (!ir_camera_info_url.empty())
{
std::string camera_name_ir = "realsense_camera_ir_" + useDeviceSerialNum;
camera_info_manager::CameraInfoManager ir_camera_info_manager(n, camera_name_ir, ir_camera_info_url);
if (ir_camera_info_manager.isCalibrated())
{
ir_camera_info = boost::make_shared<sensor_msgs::CameraInfo>(ir_camera_info_manager.getCameraInfo());
if (ir_camera_info->width != depth_stream.width || ir_camera_info->height != depth_stream.height)
{
ROS_WARN("IR image resolution does not match calibration file");
ir_camera_info.reset(new sensor_msgs::CameraInfo());
ir_camera_info->width = depth_stream.width;
ir_camera_info->height = depth_stream.height;
}
}
}
if (!ir_camera_info)
{
ir_camera_info.reset(new sensor_msgs::CameraInfo());
ir_camera_info->width = depth_stream.width;
ir_camera_info->height = depth_stream.height;
}
if(debug_depth_unit && realsense_camera_type == "Intel(R) RealSense(TM) 3D Camer")
{
getRealsenseUSBHandle(usbContext, usbHandle, useDeviceSerialNum);
if(usbContext && usbHandle)
{
printf("getRealsenseUSBHandle OK!\n");
}
}
printf("RealSense Camera is running!\n");
#if USE_BGR24
rgb_frame_buffer = new unsigned char[rgb_stream.width * rgb_stream.height * 3];
#else
rgb_frame_buffer = new unsigned char[rgb_stream.width * rgb_stream.height * 2];
#endif
depth_frame_buffer = new unsigned char[depth_stream.width * depth_stream.height];
#ifdef V4L2_PIX_FMT_INZI
ir_frame_buffer = new unsigned char[depth_stream.width * depth_stream.height];
#endif
realsense_points_pub = n.advertise<sensor_msgs::PointCloud2> (topic_depth_points_id, 1);
realsense_reg_points_pub = n.advertise<sensor_msgs::PointCloud2>(topic_depth_registered_points_id, 1);
realsense_rgb_image_pub = image_transport.advertiseCamera(topic_image_rgb_raw_id, 1);
realsense_depth_image_pub = image_transport.advertiseCamera(topic_image_depth_raw_id, 1);
//pub_depth_info = n.advertise<sensor_msgs::CameraInfo>("depth/camera_info", 1);
//pub_rgb_info = n.advertise<sensor_msgs::CameraInfo>("rgb/camera_info", 1);
#ifdef V4L2_PIX_FMT_INZI
realsense_infrared_image_pub = image_transport.advertiseCamera(topic_image_infrared_raw_id, 1);
#endif
ros::Subscriber config_sub = n.subscribe("/realsense_camera_config", 1, realsenseConfigCallback);
getRGBUVService = n.advertiseService("get_rgb_uv", getRGBUV);
capturer_mmap_init_v4l2_controls();
dynamic_reconfigure::Server<realsense_camera::RealsenseCameraConfig> dynamic_reconfigure_server;
dynamic_reconfigure_server.setCallback(boost::bind(&dynamicReconfigCallback, _1, _2));
ros::Rate loop_rate(60);
ros::Rate idle_rate(1);
while(ros::ok())
{
while ((getNumRGBSubscribers() + getNumDepthSubscribers()) == 0 && ros::ok())
{
ros::spinOnce();
idle_rate.sleep();
}
processRGBD();
#if SHOW_RGBD_FRAME
cv::waitKey(10);
#endif
ros::spinOnce();
loop_rate.sleep();
}
capturer_mmap_exit(&rgb_stream);
capturer_mmap_exit(&depth_stream);
delete[] rgb_frame_buffer;
delete[] depth_frame_buffer;
#ifdef V4L2_PIX_FMT_INZI
delete[] ir_frame_buffer;
#endif
if(debug_depth_unit)
{
libusb_close(usbHandle);
libusb_exit(usbContext);
}
printf("RealSense Camera is shutdown!\n");
return 0;
}
int main(int argc, char **argv)
{
// Set up ROS.
ros::init(argc, argv, "compass_node");
ros::NodeHandle n;
ros::NodeHandle private_node_handle_("~");
// Local variables.
int baud;
int init_time;
string portname;
string pub_topic_name;
int rate;
// Initialize node parameters.
private_node_handle_.param("baud", baud, int(115200));
private_node_handle_.param("init_time", init_time, int(3));
private_node_handle_.param("port", portname, string("/dev/os5000"));
private_node_handle_.param("pub_topic_name", pub_topic_name, string("os5000_data"));
private_node_handle_.param("rate", rate, int(50));
// Create a new OSCompass object.
OSCompass *oscompass = new OSCompass(portname, baud, rate, init_time);
// Set up a dynamic reconfigure server.
dynamic_reconfigure::Server<os5000::os5000Config> gain_srv;
dynamic_reconfigure::Server<os5000::os5000Config>::CallbackType f;
f = boost::bind(&OSCompass::configCallback, oscompass, _1, _2);
gain_srv.setCallback(f);
// SePt up publishers.
ros::Publisher pubImuData = n.advertise<sensor_msgs::Imu>(pub_topic_name.c_str(), 1);
ros::Publisher pubDepthData = n.advertise<os5000::DepthMessage>("depthMessage", 1);
// Tell ROS to run this node at the rate that the compass is sending messages to us.
ros::Rate r(rate);
// Connect to the Ocean Server compass.
if (oscompass->fd < 0)
{
ROS_ERROR("Could not connect to compass on port %s at %d baud. You can try changing the parameters using the dynamic reconfigure gui.", oscompass->portname.c_str(), oscompass->baud);
}
// Main loop.
while (n.ok())
{
// Get compass data.
if (oscompass->fd > 0)
{
oscompass->getData();
if (oscompass->yaw > 180.)
{
oscompass->yaw -= 360.;
}
// Publish the message.
oscompass->publishImuData(&pubImuData);
oscompass->publishDepth(&pubDepthData);
}
else
{
ROS_INFO("Error: compass disconnected\n");
}
ros::spinOnce();
r.sleep();
}
return 0;
} // end main()
int main(int argc, char **argv)
{
// Set up ROS.
ros::init(argc, argv, "thruster_node");
ros::NodeHandle n;
ros::NodeHandle private_node_handle_("~");
ros::Publisher pub_thruster_data = n.advertise<sbBTA252::SBBTA252Data>("thrusterData", 1000);
// Declare variables.
string portname;
int baud;
int init_time;
bool reconnect;
SBBTA252 *thruster;
// Initialize node parameters.
private_node_handle_.param("port", portname, string("/dev/ttyS0"));
private_node_handle_.param("baud", baud, int(115200));
private_node_handle_.param("init_time", init_time, int(3));
private_node_handle_.param("reconnect", reconnect, bool(false));
// Create a new SBBTA252 thruster object.
thruster = new SBBTA252(portname, baud, init_time);
// Tell ROS to run this node at the rate that the compass is sending messages to us.
ros::Rate r(100);
// Set up a dynamic reconfigure server.
dynamic_reconfigure::Server<sbBTA252::sbBTA252ParamsConfig> gain_srv;
dynamic_reconfigure::Server<sbBTA252::sbBTA252ParamsConfig>::CallbackType f;
f = boost::bind(&SBBTA252::configCallback, thruster, _1, _2);
gain_srv.setCallback(f);
// Create a subscriber.
// Name the topic, message queue, callback function with class name, and object containing callback function.
ros::Subscriber sub_message = n.subscribe("controlInputs", 1000, &SBBTA252::controlsCallback, thruster);
// Set up the thruster.
thruster->setup();
// Connect to the Seabotix BTA252
if (thruster->fd < 0)
{
ROS_ERROR("Could not connect to thrusters on port %s at %d baud. You can try changing the parameters using the dynamic reconfigure gui.", portname.c_str(), baud);
}
int loopCount = 0;
// Main loop.thruster
while (n.ok())
{
if (thruster->fd > 0)
{
// If we are in manual mode then use the reconfigure commands
if (thruster->manual)
{
ROS_ERROR("Manual mode.");
if (thruster->change_addr)
{
thruster->sendChangeAddressCommand(thruster->thrust_addr, thruster->new_addr);
}
thruster->sendSpeedCommand(1, thruster->speed_perc);
thruster->sendSpeedCommand(2, thruster->speed_perc);
thruster->sendSpeedCommand(3, thruster->speed_perc);
thruster->sendSpeedCommand(4, thruster->speed_perc);
thruster->sendSpeedCommand(5, thruster->speed_perc);
if (thruster->get_status)
{
thruster->sendReadCommand(thruster->thrust_addr);
thruster->getStatus();
}
}
else
{
// Not doing manual commands so use the planner thrust information
// Send the Pitch command
thruster->sendSpeedCommand(thruster->t_pitch, thruster->u_pitch);
if ( loopCount == 5)
{
// Send the Depth commands
thruster->sendSpeedCommand(thruster->t_left_roll, thruster->u_depth);
thruster->sendSpeedCommand(thruster->t_right_roll, thruster->u_depth);
loopCount = 0;
}
else
{
// Send the Roll commands
thruster->sendSpeedCommand(thruster->t_left_roll, thruster->u_roll);
thruster->sendSpeedCommand(thruster->t_right_roll, thruster->u_roll * -1);
loopCount++;
}
// Send the Yaw commands
thruster->sendSpeedCommand(thruster->t_left_yaw, thruster->u_yaw);
thruster->sendSpeedCommand(thruster->t_right_yaw, thruster->u_yaw * -1);
}
}
ros::spinOnce();
r.sleep();
}
return 0;
} // end main()
int main(int argc, char **argv)
{
// Set up ROS.
ros::init(argc, argv, "tracking_logger");
ros::NodeHandle n;
std::signal(SIGINT, signalHandler);
// Declare variables that can be modified by launch file or command line.
int rate;
// Initialize node parameters from launch file or command line.
// Use a private node handle so that multiple instances of the node can be run simultaneously
// while using different parameters.
ros::NodeHandle private_node_handle_("~");
private_node_handle_.param("rate", rate, int(100));
//Create Files
file_gt_persons.open("gt_persons.log", std::ios::trunc);
if (file_gt_persons.is_open())
{
ROS_INFO_STREAM("File was created " << std::endl);
file_gt_persons << "timestamp ; number of detections ; detection_ids..." << std::endl;
}
file_gt_tracks.open("gt_tracks.log", std::ios::trunc);
if (file_gt_tracks.is_open())
{
ROS_INFO_STREAM("File was created " << std::endl);
file_gt_tracks << "timestamp ; number of tracks ; track_id ; pose_x ; pose_y ..." << std::endl;
}
file_persons.open("detector_persons.log", std::ios::trunc);
if (file_persons.is_open())
{
ROS_INFO_STREAM("File was created " << std::endl);
file_persons << "timestamp ; number of detections ; detection_ids..." << std::endl;
}
file_tracks.open("tracker_tracks.log", std::ios::trunc);
if (file_tracks.is_open())
{
ROS_INFO_STREAM("File was created " << std::endl);
file_tracks << "timestamp ; number of tracks ; track_id ; pose_x ; pose_y ..." << std::endl;
}
file_compare_tracks.open("tracker_comparison.log", std::ios::trunc);
if (file_compare_tracks.is_open())
{
ROS_INFO_STREAM("File was created " << std::endl);
file_compare_tracks << "timestamp ; number of tracks ground truth ; number of tracks tracker ; min_distance track <-> gt ..." << std::endl;
}
file_compare_detections.open("detection_comparison.log", std::ios::trunc);
if (file_compare_detections.is_open())
{
ROS_INFO_STREAM("File was created " << std::endl);
file_compare_detections << "timestamp ; number of detections ground truth ; number of detections ; min_distance detection <-> gt .." << std::endl;
}
// Create a subscriber.
// Name the topic, message queue, callback function with class name, and object containing callback function.
message_filters::Subscriber<spencer_tracking_msgs::DetectedPersons> sub_gt_persons(n,"/groundtruth/detected_persons", 1000);
sub_gt_persons.registerCallback(boost::bind(&callback_gt_persons, _1));
message_filters::Subscriber<spencer_tracking_msgs::TrackedPersons> sub_gt_tracks(n,"/groundtruth/tracked_persons", 1000);
sub_gt_tracks.registerCallback(boost::bind(&callback_gt_tracks, _1));
message_filters::Subscriber<spencer_tracking_msgs::DetectedPersons> sub_persons(n,"/spencer/perception/detected_persons", 1000);
sub_persons.registerCallback(boost::bind(&callback_persons, _1));
message_filters::Subscriber<spencer_tracking_msgs::TrackedPersons> sub_tracks(n,"/spencer/perception/tracked_persons", 1000);
sub_tracks.registerCallback(boost::bind(&callback_tracks, _1));
typedef message_filters::sync_policies::ApproximateTime<spencer_tracking_msgs::TrackedPersons, spencer_tracking_msgs::TrackedPersons> MySyncPolicy;
// ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy
message_filters::Synchronizer<MySyncPolicy> sync_tracks(MySyncPolicy(100),sub_gt_tracks, sub_tracks);
sync_tracks.registerCallback(boost::bind(&compare_tracks, _1, _2));
message_filters::TimeSynchronizer<spencer_tracking_msgs::DetectedPersons, spencer_tracking_msgs::DetectedPersons> sync_detections(sub_gt_persons, sub_persons, 1);
sync_detections.registerCallback(boost::bind(&compare_detections, _1, _2));
// Tell ROS how fast to run this node.
ros::Rate r(rate);
// Main loop.
while (n.ok())
{
ros::spinOnce();
r.sleep();
}
return 0;
} // end main()
int main(int argc, char **argv)
{
//! # Algorithm structure
//! ## Initialization
ros::init(argc, argv, "rollo_control");
ros::start();
//! - Initialize nodehandle for publisher
ros::NodeHandle RolloControlNode;
//! - Publisher initialization with topic, message format and queue size definition
ros::Publisher RolloTwist = RolloControlNode.advertise<geometry_msgs::Twist>(TopicCmdVel, 1024);
//! - Node arguments using command line
int rate_frequency;
//! - Initialize node parameters from launch file or command line.
//! Use a private node handle so that multiple instances of the node can be run simultaneously
//! while using different parameters.
ros::NodeHandle private_node_handle_("~");
private_node_handle_.param("rate", rate_frequency, int(100));
//! - Publishing rate [Hz]
ros::Rate frequency(rate_frequency);
//! - Publisher variables for conventional messages
geometry_msgs::Twist PubRolloCmd;
//! - Initialize variables for computing linear and angular velocity of the robot
double Speed = 0;
double Turn = 0;
double LastTurn = 0;
//! - Initialize character holder
char c = 0;
//! ## Main loop
while (ros::ok()) {
//! - Check if a key is pressed:
//! - Read character
//! - Decode key pressed
if (kbhit()) {
// Read character
c = getchar();
// Decode key pressed
decodeKey(c, Speed, Turn, LastTurn);
}
//! - Prepare message to publish linear and angular velocities
PubRolloCmd.linear.x = Speed;
PubRolloCmd.angular.z = Turn;
//! - Print message with velocities
ROS_INFO("[Rollo][%s][Pub] Linear speed [%f] Angular speed [%f]", NodeName, PubRolloCmd.linear.x, PubRolloCmd.angular.z);
//! - Publish message in Twist format
RolloTwist.publish(PubRolloCmd);
//! - ROS spinOnce
ros::spinOnce();
//! - Sleep to conform node frequency rate
frequency.sleep();
}
//! ## Main loop end
return 0;
}
int main(int argc, char *argv[])
{
ros::init(argc, argv, "scanner2d");
scanner2d::Scanner2d scanner;
std::string port_name;
int scan_rate;
int sample_rejection;
int samples_per_scan;
int min_angle;
int max_angle;
ROS_DEBUG("Welcome to scanner2d Node!");
signal(SIGINT, sig_handler);
ros::NodeHandle private_node_handle_("~");
private_node_handle_.param("port_name", port_name, std::string("/dev/ttyACM0"));
private_node_handle_.param("scan_rate", scan_rate, int(3));
private_node_handle_.param("samples_per_scan", samples_per_scan, int(333));
private_node_handle_.param("sample_rejection", sample_rejection, int(0));
private_node_handle_.param("min_angle", min_angle, int(0));
private_node_handle_.param("max_angle", max_angle, int(360));
if (scan_rate*samples_per_scan > 1000) {
ROS_WARN("The scan_rate * samples_per_scan exceeds the max sample rate (1000) of the sensor!");
ROS_WARN(" you should either alter the settings from the command line invocation,");
ROS_WARN(" or reconfigure the parameter server directly.");
}
scanner.open(port_name);
switch(scan_rate)
{
case 1:
scanner.setScanPeriod(1000);
break;
case 2:
scanner.setScanPeriod(500);
break;
case 3:
scanner.setScanPeriod(333);
break;
case 4:
scanner.setScanPeriod(250);
break;
case 5:
scanner.setScanPeriod(200);
break;
default:
ROS_WARN("Invalid scan_rate!");
}
scanner.setSampleRejectionMode((bool)sample_rejection);
scanner.setSamplesPerScan(samples_per_scan);
scanner.setMinMaxAngle(min_angle, max_angle);
ros::Rate loop_rate(10);
while (ros::ok())
{
scanner2d::Scanner2dStatus_t status;
scanner.getStatus(&status);
if (status.flags & 0x80)
{
ROS_DEBUG("Got quit from scanner");
ROS_DEBUG(" Status flags: 0x%x", status.flags);
break;
}
if (got_ctrl_c) {
ROS_WARN("Got Ctrl-C");
scanner.stop();
ros::Duration(0.5).sleep();
scanner.close();
break;
}
ros::spinOnce();
loop_rate.sleep();
}
ROS_WARN("Exiting.");
exit(0);
}
int main(int argc, char **argv){
// Set up ROS.
ros::init(argc, argv, "IMU_republisher");
ros::NodeHandle n;
// Declare variables that can be modified by launch file or command line.
int rate;
bool simulator;
string topic;
sleep(5);
// Create a new IMU_replublish object.
IMU_replublish *IMU_node_handler = new IMU_replublish();
// Initialize node parameters from launch file or command line.
// Use a private node handle so that multiple instances of the node can be run simultaneously
// while using different parameters.
ros::NodeHandle private_node_handle_("~");
private_node_handle_.param("rate", rate, int(40));
private_node_handle_.param("not_sim", simulator , bool(true));
private_node_handle_.param("topic", topic, string("/mavros/imu/data"));
// Create a subscriber.
// Name the topic, message queue, callback function with class name, and object containing callback function.
ros::Subscriber sub_message = n.subscribe(topic.c_str(), 1000, &IMU_replublish::IMU_data_messageCallback, IMU_node_handler);
ros::Subscriber sub_message1 = n.subscribe("/mavros/imu/mag", 1000, &IMU_replublish::IMU_MAG_data_messageCallback, IMU_node_handler);
ros::Subscriber sub_message2 = n.subscribe("/mavros/global_position/compass_hdg", 1000,&IMU_replublish::IMU_data_yawmag_messageCallback, IMU_node_handler);
ros::Subscriber sub_message3 = n.subscribe("/mavros/global_position/raw/gps_vel", 1000,&IMU_replublish::IMU_yawfromGPSVelocity_messageCallback, IMU_node_handler);
IMU_node_handler->pub_message_IMU = n.advertise<sensor_msgs::Imu>("imu_data_ENU", 40);
// Tell ROS how fast to run this node.
ros::Rate r(rate);
ros::ServiceClient client = n.serviceClient<mavros_msgs::StreamRateRequest>("/mavros/set_stream_rate");
mavros_msgs::StreamRate srv;
srv.request.message_rate=20;
srv.request.stream_id=0;
srv.request.on_off=1;
// if the system is not running in simulation the service mavros is not used
if(simulator){
ros::service::waitForService("/mavros/set_stream_rate", -1);
ROS_INFO("MAVROS SERVICE is available..");
if (client.call(srv)){
ROS_INFO("Configure the MAVROS RATE");
std::cout << topic.c_str() << std::endl;
}else{
ROS_ERROR("Failed to call service /mavros/set_stream_rate");
return 1;
}
}
// Main loop.
int n_call=0;
while (n.ok()){
ros::spinOnce();
r.sleep();
if(IMU_node_handler->n_msg < 20){
n_call++;
if(n_call > 20){
n_call=0;
if(simulator){
if (client.call(srv)){
ROS_INFO("Configure the MAVROS RATE");
}else{
ROS_ERROR("Failed to call service /mavros/set_stream_rate");
}
}
}
}
}
return 0;
} // end main()
