int main(int argc, char** argv) {
ros::init(argc, argv, "message_to_tf");
g_footprint_frame_id = "base_footprint";
g_stabilized_frame_id = "base_stabilized";
g_position_frame_id = "base_position";
ros::NodeHandle priv_nh("~");
priv_nh.getParam("odometry_topic", g_odometry_topic);
priv_nh.getParam("pose_topic", g_pose_topic);
priv_nh.getParam("imu_topic", g_imu_topic);
priv_nh.getParam("frame_id", g_frame_id);
priv_nh.getParam("footprint_frame_id", g_footprint_frame_id);
priv_nh.getParam("position_frame_id", g_position_frame_id);
priv_nh.getParam("stabilized_frame_id", g_stabilized_frame_id);
priv_nh.getParam("child_frame_id", g_child_frame_id);
br = new tf::TransformBroadcaster;
ros::NodeHandle node;
ros::Subscriber sub1, sub2, sub3;
if (!g_odometry_topic.empty()) sub1 = node.subscribe(g_odometry_topic, 10, &odomCallback);
if (!g_pose_topic.empty()) sub2 = node.subscribe(g_pose_topic, 10, &poseCallback);
if (!g_imu_topic.empty()) sub3 = node.subscribe(g_imu_topic, 10, &imuCallback);
if (!sub1 && !sub2 && !sub3) {
ROS_FATAL("Params odometry_topic, pose_topic and imu_topic are empty... nothing to do for me!");
return 1;
}
ros::spin();
delete br;
return 0;
}
int main(int argc, char *argv[])
{
ros::init(argc, argv, "copter_visualization");
ros::NodeHandle nh;
ros::NodeHandle priv_nh("~");
int num_rotors;
double arm_len, body_width, body_height;
priv_nh.param<std::string>("fixed_frame_id", fixed_frame_id, "local_origin");
priv_nh.param<std::string>("child_frame_id", child_frame_id, "fcu");
priv_nh.param("marker_scale", marker_scale, 1.0);
priv_nh.param("num_rotors", num_rotors, 6);
priv_nh.param("arm_len", arm_len, 0.22 );
priv_nh.param("body_width", body_width, 0.15 );
priv_nh.param("body_height", body_height, 0.10 );
priv_nh.param("max_track_size", max_track_size, 1000 );
create_vehicle_markers( num_rotors, arm_len, body_width, body_height );
track_marker_pub = nh.advertise<visualization_msgs::Marker>("track_markers", 10);
vehicle_marker_pub = nh.advertise<visualization_msgs::MarkerArray>("vehicle_marker", 10);
wp_marker_pub = nh.advertise<visualization_msgs::Marker>("wp_markers", 10);
auto pos_sub = nh.subscribe("local_position", 10, local_position_sub_cb);
auto wp_sub = nh.subscribe("local_setpoint", 10, setpoint_local_pos_sub_cb);
ros::spin();
return 0;
}
serial_server(const ros::NodeHandle& node_handle)
: server_(node_handle, &serial_)
{
ros::NodeHandle priv_nh("~");
priv_nh.param("port", port_, std::string("/dev/ttyUSB0"));
priv_nh.param("baudrate", baudrate_, 57600);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "velodyne_conversion", ros::init_options::AnonymousName);
ros::NodeHandle nh, priv_nh("~");
stdr_velodyne::ConversionNodeBase conv(nh, priv_nh);
ros::spin();
return 0;
}
ServoStatePublisher() :
nh()
{
ros::NodeHandle priv_nh("~");
XmlRpc::XmlRpcValue param_dict;
priv_nh.getParam("", param_dict);
ROS_ASSERT(param_dict.getType() == XmlRpc::XmlRpcValue::TypeStruct);
urdf::Model model;
model.initParam("robot_description");
ROS_INFO("SSP: URDF robot: %s", model.getName().c_str());
for (auto &pair : param_dict) {
ROS_DEBUG("SSP: Loading joint: %s", pair.first.c_str());
// inefficient, but easier to program
ros::NodeHandle pnh(priv_nh, pair.first);
bool rc_rev;
int rc_channel, rc_min, rc_max, rc_trim, rc_dz;
if (!pnh.getParam("rc_channel", rc_channel)) {
ROS_ERROR("SSP: '%s' should provice rc_channel", pair.first.c_str());
continue;
}
pnh.param("rc_min", rc_min, 1000);
pnh.param("rc_max", rc_max, 2000);
if (!pnh.getParam("rc_trim", rc_trim)) {
rc_trim = rc_min + (rc_max - rc_min) / 2;
}
pnh.param("rc_dz", rc_dz, 0);
pnh.param("rc_rev", rc_rev, false);
auto joint = model.getJoint(pair.first);
if (!joint) {
ROS_ERROR("SSP: URDF: there no joint '%s'", pair.first.c_str());
continue;
}
if (!joint->limits) {
ROS_ERROR("SSP: URDF: joint '%s' should provide <limit>", pair.first.c_str());
continue;
}
double lower = joint->limits->lower;
double upper = joint->limits->upper;
servos.emplace_back(pair.first, lower, upper, rc_channel, rc_min, rc_max, rc_trim, rc_dz, rc_rev);
ROS_INFO("SSP: joint '%s' (RC%d) loaded", pair.first.c_str(), rc_channel);
}
rc_out_sub = nh.subscribe("rc_out", 10, &ServoStatePublisher::rc_out_cb, this);
joint_states_pub = nh.advertise<sensor_msgs::JointState>("joint_states", 10);
}
void QuadDecodeMsg::onInit(void)
{
ros::NodeHandle priv_nh(getPrivateNodeHandle());
serial_sub_ = priv_nh.subscribe("serial", 10, &QuadDecodeMsg::serial_callback, this,
ros::TransportHints().tcpNoDelay());
output_data_pub_ = priv_nh.advertise<quadrotor_msgs::OutputData>("output_data", 10);
imu_output_pub_ = priv_nh.advertise<sensor_msgs::Imu>("imu", 10);
status_pub_ = priv_nh.advertise<quadrotor_msgs::StatusData>("status", 10);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "octomap_creator");
ros::NodeHandle nh;
tf_listener = new tf::TransformListener();
ros::Publisher occupied_pub = nh.advertise<octomap_msgs::Octomap>("/octomap", 1, true);
ros::NodeHandle priv_nh("~");
double resolution;
priv_nh.param<double>("resolution", resolution, 0.1);
ROS_INFO("Creating tree with resolution %3.3f", resolution);
tree = new octomap::OcTree(resolution);
ros::Subscriber laser_sub = nh.subscribe<sensor_msgs::PointCloud2>("/laser/points", 100, &laserPointsCallback);
octomap_msgs::Octomap octomap_msg;
octomap_msg.header.frame_id = "/map";
ros::Rate loop_rate(1);
while(ros::ok())
{
if(map_changed)
{
ROS_DEBUG("preparing to publish data");
ROS_DEBUG("tree size: %lu", tree->size());
// this is necessary because the callback uses lazy evaluation
tree->updateInnerOccupancy();
// convert to the ros message type
octomap_msg.data.clear();
octomap_msgs::fullMapToMsg(*tree, octomap_msg);
if(octomap_msg.data.size() > 0)
{
ROS_DEBUG("Publishing octomap");
octomap_msg.header.stamp = ros::Time::now();
occupied_pub.publish(octomap_msg);
}
map_changed = false;
}
ros::spinOnce();
loop_rate.sleep();
}
delete tf_listener;
delete tree;
}
/** Nodelet initialization.
*
* @note MUST return immediately.
*/
void MVCameraNodelet::onInit()
{
ros::NodeHandle priv_nh(getPrivateNodeHandle());
ros::NodeHandle node(getNodeHandle());
ros::NodeHandle camera_nh(node, "camera");
dvr_.reset(new mv_camera::MVCameraDriver(priv_nh, camera_nh));
dvr_->setup();
// spawn device thread
running_ = true;
deviceThread_ = boost::shared_ptr< boost::thread >
(new boost::thread(boost::bind(&MVCameraNodelet::devicePoll, this)));
}
/** Nodelet initialization.
*
* @note MUST return immediately.
*/
void Bumblebee1394Nodelet::onInit()
{
ros::NodeHandle priv_nh(getPrivateNodeHandle());
ros::NodeHandle node(getNodeHandle());
ros::NodeHandle camera_nh(node, "bumblebee");
dvr_.reset(new bumblebee1394_driver::Bumblebee1394Driver(priv_nh, camera_nh));
dvr_->setup();
// spawn device thread
running_ = true;
deviceThread_ = boost::shared_ptr< boost::thread >
(new boost::thread(boost::bind(&Bumblebee1394Nodelet::devicePoll, this)));
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "qrcode_detection");
ros::NodeHandle node;
ros::NodeHandle priv_nh("~");
// create QRCodeDetection class
qrcode_detection::QRCodeDetection n(node, priv_nh);
// handle callbacks until shut down
ros::spin();
return 0;
}
/** Main entry point. */
int main(int argc, char **argv)
{
ros::init(argc, argv, "heightmap_node");
ros::NodeHandle node;
ros::NodeHandle priv_nh("~");
// create height map class, which subscribes to velodyne_points
velodyne_height_map::HeightMap hm(node, priv_nh);
// handle callbacks until shut down
ros::spin();
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "camera");
ros::NodeHandle node;
ros::NodeHandle priv_nh("~");
// create StereoNode class
ueye::StereoNode n(node, priv_nh);
// handle callbacks until shut down
ros::spin();
return 0;
}
udp_server(const ros::NodeHandle& node_handle)
: server_(node_handle, &socket_)
{
ros::NodeHandle priv_nh("~");
priv_nh.param("bind_address", local_addr_, std::string());
priv_nh.param("port", local_port_, 8090);
bool ipv6;
priv_nh.param("ipv6", ipv6, false);
if (ipv6)
local_endpoint_ = socket::Udp::Endpoint(boost::asio::ip::udp::v6(), local_port_);
else
local_endpoint_ = socket::Udp::Endpoint(boost::asio::ip::udp::v4(), local_port_);
if (!local_addr_.empty()) local_endpoint_.address(boost::asio::ip::address::address::from_string("aaaa::c30c:0:0:ffff"));
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "neato_laser_publisher");
ros::NodeHandle n;
ros::NodeHandle priv_nh("~");
signal(SIGABRT,handleTerm);
signal(SIGTERM,handleTerm);
signal(SIGINT, handleTerm);
std::string port;
int baud_rate;
std::string frame_id;
int firmware_number;
std_msgs::UInt16 rpms;
priv_nh.param("port", port, std::string("/dev/ttyACM0"));
priv_nh.param("baud_rate", baud_rate, 115200);
priv_nh.param("frame_id", frame_id, std::string("laser"));
priv_nh.param("firmware_version", firmware_number, 1);
boost::asio::io_service io;
try {
laser = new xv_11_laser_driver::XV11Laser(port, baud_rate, firmware_number, io);
ros::Publisher laser_pub = n.advertise<sensor_msgs::LaserScan>("scan", 1000);
ros::Publisher motor_pub = n.advertise<std_msgs::UInt16>("rpms",1000);
while (ros::ok()) {
sensor_msgs::LaserScan::Ptr scan(new sensor_msgs::LaserScan);
scan->header.frame_id = frame_id;
scan->header.stamp = ros::Time::now();
laser->poll(scan);
rpms.data=laser->rpms;
laser_pub.publish(scan);
motor_pub.publish(rpms);
}
laser->close();
return 0;
} catch (boost::system::system_error ex) {
ROS_ERROR("Error instantiating laser object. Are you sure you have the correct port and baud rate? Error was %s", ex.what());
return -1;
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "neato_laser_publisher");
if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug) ) {
ros::console::notifyLoggerLevelsChanged();
}
ros::NodeHandle n;
ros::NodeHandle priv_nh("~");
std::string port;
int baud_rate;
std::string frame_id;
int firmware_number;
std_msgs::UInt16 rpms;
priv_nh.param("port", port, std::string("/dev/ttyUSB0"));
priv_nh.param("baud_rate", baud_rate, 115200);
priv_nh.param("frame_id", frame_id, std::string("neato_laser"));
priv_nh.param("firmware_version", firmware_number, 2);//default to the new firmware
boost::asio::io_service io;
try {
xv_11_laser_driver::XV11Laser laser(port, baud_rate, firmware_number, io);
ros::Publisher laser_pub = n.advertise<sensor_msgs::LaserScan>("scan", 1000);
ros::Publisher motor_pub = n.advertise<std_msgs::UInt16>("rpms",1000);
ROS_DEBUG("Running XV-11 lidar publisher");
while (ros::ok()) {
sensor_msgs::LaserScan::Ptr scan(new sensor_msgs::LaserScan);
scan->header.frame_id = frame_id;
scan->header.stamp = ros::Time::now();
laser.poll(scan);
rpms.data=laser.rpms;
laser_pub.publish(scan);
motor_pub.publish(rpms);
}
laser.close();
return 0;
} catch (boost::system::system_error ex) {
ROS_DEBUG("stopping...");
ROS_ERROR("Error instantiating laser object. Are you sure you have the correct port and baud rate? Error was %s", ex.what());
return -1;
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "octomap_saver");
ros::NodeHandle nh;
ros::NodeHandle priv_nh("~");
std::string output_file;
priv_nh.param<std::string>("output_file", output_file, "~/octomap.bt");
octomap_sub = nh.subscribe<octomap_msgs::Octomap>("/octomap", 1, &octomapCallback);
ROS_INFO("Waiting for octomap");
ros::Rate loop_rate(10);
while(ros::ok() && octomap_msg == NULL)
{
ros::spinOnce();
loop_rate.sleep();
}
if(!ros::ok())
{
ROS_INFO("Quitting");
return 1;
}
ROS_INFO("Deserialzing octomap");
octomap::AbstractOcTree* tree = octomap_msgs::msgToMap(*octomap_msg);
octomap::OcTree* octree = dynamic_cast<octomap::OcTree*>(tree);
if (octree == NULL)
{
ROS_ERROR("Failed to deserialize octomap");
return 1;
}
ROS_INFO("Saving octomap to %s", output_file.c_str());
std::ofstream file(output_file.c_str());
octree->writeBinary(file);
ROS_INFO("Success!");
}
/** Main node entry point. */
int main(int argc, char **argv)
{
ros::init(argc, argv, "camera1394_node");
ros::NodeHandle node;
ros::NodeHandle priv_nh("~");
ros::NodeHandle camera_nh("camera");
signal(SIGSEGV, &sigsegv_handler);
camera1394_driver::Camera1394Driver dvr(priv_nh, camera_nh);
dvr.setup();
while (node.ok())
{
dvr.poll();
ros::spinOnce();
}
dvr.shutdown();
return 0;
}
int main(int argc, char *argv[])
{
ros::init(argc, argv, "faro_data_procesing_node");
ros::NodeHandle nh;
ros::NodeHandle priv_nh("~");
//GCRecognitionNode rec(nh);
//rec.init();
//rec.spin();
init(nh, priv_nh);
//process();
//while (ros::ok())
//{
process();
//}
//ros::spin();
//return 0;
}
int main(int argc, char** argv){
ros::init(argc, argv, "assisted_drive");
ros::NodeHandle nh;
ros::NodeHandle priv_nh("~");
std::string source_topic, target_topic, costmap_topic;
try {
priv_nh.getParam("source_topic", source_topic);
priv_nh.getParam("target_topic", target_topic);
priv_nh.getParam("costmap_topic", costmap_topic);
priv_nh.getParam("max_range", MAX_RANGE);
priv_nh.getParam("clearing_dist", CLEARING_DIST);
priv_nh.getParam("base_frame_id", BASE_FRAME);
double diameter;
nh.getParam("base_diameter", diameter);
BASE_RADIUS = diameter / 2;
ROS_ASSERT(MAX_RANGE > BASE_RADIUS + CLEARING_DIST);
}
catch (ros::Exception e) {
ROS_ERROR("Parameter not set: %s", e.what());
return 1;
}
tf_listener = new tf::TransformListener(nh);
ros::Subscriber twist_sub = nh.subscribe<geometry_msgs::Twist>(source_topic, 10, twist_cb);
ros::Subscriber costmap_sub = nh.subscribe<nav_msgs::GridCells>(costmap_topic, 10, costmap_cb);
twist_pub = nh.advertise<geometry_msgs::Twist>(target_topic, 10);
force_field_pub = nh.advertise<geometry_msgs::PoseArray>("force_field", 10);
force_obst_pub = nh.advertise<geometry_msgs::PoseStamped>("force_obst", 10);
force_twist_pub = nh.advertise<geometry_msgs::PoseStamped>("force_twist", 10);
result_twist_pub = nh.advertise<geometry_msgs::PoseStamped>("result_twist", 10);
ros::spin();
return 0;
}
int main(int argc, char** argv) {
ros::init(argc, argv, "message_to_tf");
g_footprint_frame_id = "base_footprint";
g_stabilized_frame_id = "base_stabilized";
// g_position_frame_id = "base_position";
// g_child_frame_id = "base_link";
ros::NodeHandle priv_nh("~");
priv_nh.getParam("odometry_topic", g_odometry_topic);
priv_nh.getParam("pose_topic", g_pose_topic);
priv_nh.getParam("imu_topic", g_imu_topic);
priv_nh.getParam("topic", g_topic);
priv_nh.getParam("frame_id", g_frame_id);
priv_nh.getParam("footprint_frame_id", g_footprint_frame_id);
priv_nh.getParam("position_frame_id", g_position_frame_id);
priv_nh.getParam("stabilized_frame_id", g_stabilized_frame_id);
priv_nh.getParam("child_frame_id", g_child_frame_id);
g_tf_prefix = tf::getPrefixParam(priv_nh);
g_transform_broadcaster = new tf::TransformBroadcaster;
ros::NodeHandle node;
ros::Subscriber sub1, sub2, sub3, sub4;
if (!g_odometry_topic.empty()) sub1 = node.subscribe(g_odometry_topic, 10, &odomCallback);
if (!g_pose_topic.empty()) sub2 = node.subscribe(g_pose_topic, 10, &poseCallback);
if (!g_imu_topic.empty()) sub3 = node.subscribe(g_imu_topic, 10, &imuCallback);
if (!g_topic.empty()) sub4 = node.subscribe(g_topic, 10, &multiCallback);
if (!sub1 && !sub2 && !sub3 && !sub4) {
ROS_FATAL("Params odometry_topic, pose_topic, imu_topic and topic are empty... nothing to do for me!");
return 1;
}
bool publish_pose = true;
priv_nh.getParam("publish_pose", publish_pose);
if (publish_pose) {
std::string publish_pose_topic;
priv_nh.getParam("publish_pose_topic", publish_pose_topic);
if (!publish_pose_topic.empty())
g_pose_publisher = node.advertise<geometry_msgs::PoseStamped>(publish_pose_topic, 10);
else
g_pose_publisher = priv_nh.advertise<geometry_msgs::PoseStamped>("pose", 10);
}
bool publish_euler = true;
priv_nh.getParam("publish_euler", publish_euler);
if (publish_euler) {
std::string publish_euler_topic;
priv_nh.getParam("publish_euler_topic", publish_euler_topic);
if (!publish_euler_topic.empty())
g_euler_publisher = node.advertise<geometry_msgs::Vector3Stamped>(publish_euler_topic, 10);
else
g_euler_publisher = priv_nh.advertise<geometry_msgs::Vector3Stamped>("euler", 10);
}
ros::spin();
delete g_transform_broadcaster;
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "robot_sim");
ros::NodeHandle priv_nh("~");
urdf::Model model;
std::map<std::string, size_t> name_map;
size_t i=0;
if (!model.initParam("robot_description"))
{
ROS_ERROR("Robot sim bringup: failed to read urdf from parameter server");
return 0;
}
for (std::map<std::string, boost::shared_ptr<urdf::Joint> >::iterator it = model.joints_.begin();
it != model.joints_.end(); it++)
{
if (it->second->type != urdf::Joint::FIXED)
{
name_map[it->first]=i;
i++;
}
}
ROS_INFO_STREAM("Robot model read with " << name_map.size() << " non-fixed joints.");
size_t num_joints = name_map.size();
boost::shared_ptr<robot_sim::Robot> robot(new robot_sim::Robot(name_map));
robot->init();
boost::shared_ptr<robot_sim::JointStatePublisher> publisher(new robot_sim::JointStatePublisher(robot));
double rate;
priv_nh.param<double>("joint_pub_rate", rate, 0.1);
if (!publisher->init(rate))
{
ROS_ERROR("Failed to initialize publisher");
return 0;
}
boost::shared_ptr<robot_sim::VelocityController> vel_controller(new robot_sim::VelocityController(robot));
if (!vel_controller->init())
{
ROS_ERROR("Failed to initialize velocity controller");
return 0;
}
boost::shared_ptr<robot_sim::PositionController> pos_controller(new robot_sim::PositionController(robot));
if (!pos_controller->init())
{
ROS_ERROR("Failed to initialize position controller");
return 0;
}
boost::shared_ptr<robot_sim::PositionCommand> pos_command(new robot_sim::PositionCommand(robot));
if (!pos_command->init())
{
ROS_ERROR("Failed to initialize position command");
return 0;
}
ros::NodeHandle root_nh("");
//ros::Timer timer = root_nh.createTimer(ros::Duration(1.0), boost::bind(setVelocities, robot, num_joints));
ROS_INFO("Simulated robot spinning");
ros::spin();
}
