int main(int argc, char **argv){
ros::init(argc, argv, "apriltag_detector");
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
apriltags_ros::AprilTagDetector detector(nh, pnh);
ros::spin();
}
Marker6DOF(): show_6dof_circle_(true) {
ros::NodeHandle nh, pnh("~");
pnh.param("frame_id", frame_id_, std::string("/map"));
pnh.param("object_type", object_type_, std::string("sphere"));
pnh.param("object_x", object_x_, 1.0);
pnh.param("object_y", object_y_, 1.0);
pnh.param("object_z", object_z_, 1.0);
pnh.param("object_r", object_r_, 1.0);
pnh.param("object_g", object_g_, 1.0);
pnh.param("object_b", object_b_, 1.0);
pnh.param("object_a", object_a_, 1.0);
pnh.param("line_width", line_width_, 0.007);
latest_pose_.header.frame_id = frame_id_;
latest_pose_.pose.orientation.w = 1.0;
pose_pub_ = pnh.advertise<geometry_msgs::PoseStamped>("pose", 1);
pose_stamped_sub_ = pnh.subscribe("move_marker", 1, &Marker6DOF::moveMarkerCB, this);
circle_menu_entry_
= menu_handler_.insert("Toggle 6DOF Circle",
boost::bind(&Marker6DOF::menuFeedbackCB, this, _1));
menu_handler_.setCheckState(circle_menu_entry_,
interactive_markers::MenuHandler::CHECKED);
server_.reset( new interactive_markers::InteractiveMarkerServer(ros::this_node::getName()));
initializeInteractiveMarker();
}
LaserScanFilter()
{
ros::NodeHandle nh;
scan_sub_ = nh.subscribe("hokuyo_scan", 1, &LaserScanFilter::scanCallback, this);
scan_filtered_pub_ = nh.advertise<sensor_msgs::LaserScan>("hokuyo_scan_filtered",1,false);
ros::NodeHandle pnh("~");
XmlRpc::XmlRpcValue my_list;
pnh.getParam("filter_index_list", my_list);
if (my_list.getType() != XmlRpc::XmlRpcValue::TypeArray) ros::shutdown();
for (int32_t i = 0; i < my_list.size(); ++i)
{
if (my_list[i].getType() != XmlRpc::XmlRpcValue::TypeArray) ros::shutdown();
int min = my_list[i][0];
int max = my_list[i][1];
addFilterIndices(min, max);
ROS_INFO("scan filter index interval %d : min: %d max: %d",i, min, max);
}
}
Footcoords::Footcoords():
diagnostic_updater_(new diagnostic_updater::Updater)
{
ros::NodeHandle nh, pnh("~");
lforce_list_.resize(0);
rforce_list_.resize(0);
tf_listener_.reset(new tf::TransformListener());
ground_transform_.setRotation(tf::Quaternion(0, 0, 0, 1));
midcoords_.setRotation(tf::Quaternion(0, 0, 0, 1));
diagnostic_updater_->setHardwareID("none");
diagnostic_updater_->add("Support Leg Status", this,
&Footcoords::updateLegDiagnostics);
// read parameter
pnh.param("alpha", alpha_, 0.5);
pnh.param("sampling_time_", sampling_time_, 0.2);
pnh.param("output_frame_id", output_frame_id_,
std::string("odom_on_ground"));
pnh.param("parent_frame_id", parent_frame_id_, std::string("odom"));
pnh.param("midcoords_frame_id", midcoords_frame_id_, std::string("ground"));
pnh.param("lfoot_frame_id", lfoot_frame_id_,
std::string("lleg_end_coords"));
pnh.param("rfoot_frame_id", rfoot_frame_id_,
std::string("rleg_end_coords"));
pnh.param("force_threshold", force_thr_, 100.0);
pub_state_ = pnh.advertise<std_msgs::String>("state", 1);
pub_contact_state_ = pnh.advertise<jsk_footstep_controller::GroundContactState>("contact_state", 1);
before_on_the_air_ = true;
sub_lfoot_force_.subscribe(nh, "lfsensor", 1);
sub_rfoot_force_.subscribe(nh, "rfsensor", 1);
sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
sync_->connectInput(sub_lfoot_force_, sub_rfoot_force_);
sync_->registerCallback(boost::bind(&Footcoords::filter, this, _1, _2));
}
int main(int argc, char **argv)
{
// Set up ROS.
ros::init(argc, argv, "listener");
ros::NodeHandle nh;
// 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 pnh("~");
pnh.param("rate", rate, int(40));
// Create a new NodeExample object.
//NodeExample *node_example = new NodeExample();
// Create a subscriber.
// Name the topic, message queue, callback function with class name, and object containing callback function.
ros::Subscriber sub_message = nh.subscribe("example", 1000, &messageCallback);
// Tell ROS how fast to run this node.
ros::Rate r(rate);
// Main loop.
while (nh.ok())
{
ros::spinOnce();
r.sleep();
}
return 0;
} // end main()
int main(int argc, char **argv) {
ros::init(argc, argv, "oryx_diagnostics_client");
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
ros::ServiceClient client = nh.serviceClient<oryx_diagnostics::DiagnosticsCommand>("oryx/diagnostics/commands");
std::string command;
std::string arguments;
pnh.getParam("command", command);
pnh.getParam("arg", arguments);
if(command == "") ROS_WARN("No Command Given!");
else{
oryx_diagnostics::DiagnosticsCommand msg;
msg.request.command = command;
msg.request.data = arguments;
if(client.call(msg.request, msg.response)){
if(msg.response.success)
{
ROS_INFO("%s", msg.response.data.c_str());
}
else
{
ROS_WARN("Error With Command: %s", msg.response.data.c_str());
}
}
else
{
ROS_ERROR("Could Not Communicate With Server!");
}
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "pid_listener");
ros::NodeHandle nh;
int rate;
ros::NodeHandle pnh("~");
pnh.param("rate", rate, int(40));
LinoPID *lino_pid = new LinoPID();
ros::Subscriber sub_message = nh.subscribe("pid", 1000, &LinoPID::messageCallback, lino_pid);
ros::Rate r(rate);
// Main loop.
while (nh.ok())
{
ros::spinOnce();
r.sleep();
}
return 0;
} // end main()
PointCloudProjector::PointCloudProjector(ros::NodeHandle nh) :
pointcloud_sub(nh, "pointcloud", 1),
patch_sub(nh, "patches", 1),
sync(ExactSyncPolicy(50), pointcloud_sub, patch_sub)
{
patches_out = nh.advertise<samplereturn_msgs::PatchArray>("positioned_patches", 1);
debug_points_out = nh.advertise<sensor_msgs::PointCloud2>("debug_pointcloud", 1);
debug_marker_out = nh.advertise<visualization_msgs::MarkerArray>("debug_marker", 1);
sync.registerCallback(boost::bind(&PointCloudProjector::synchronized_callback, this, _1, _2));
ros::NodeHandle pnh("~");
pnh.param("clipping_frame_id", clipping_frame_id_, std::string("base_link"));
pnh.param("output_frame_id", output_frame_id_, std::string("odom"));
if(pnh.param("start_enabled", true))
{
enabled_ = true;
pointcloud_sub.subscribe(nh, "pointcloud", 1);
patch_sub.subscribe(nh, "patches", 1);
}
else
{
enabled_ = false;
}
reconfigure_server.setCallback(boost::bind(&PointCloudProjector::configure_callback, this, _1, _2));
enable_service = pnh.advertiseService( "enable", &PointCloudProjector::enable, this);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "image_publisher");
ros::NodeHandle nh, pnh("~");
FakeCameraParams params = loadParams(pnh);
image_transport::ImageTransport it(nh);
image_transport::Publisher pub =
it.advertise(params.topic+"/image", 1);
ros::Publisher info_pub =
nh.advertise<sensor_msgs::CameraInfo>(params.topic+"/camera_info", 1);
sensor_msgs::CameraInfo info = loadCameraInfo(params);
cv::Mat image =
cv::Mat(params.width, params.height, CV_8UC3, cv::Scalar(50,50,50));
sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
msg->header.frame_id = params.frame;
msg->header.stamp = ros::Time::now();
ros::Rate loop_rate(params.rate);
while (nh.ok()) {
msg->header.stamp = ros::Time::now();
info.header.stamp = ros::Time::now();
pub.publish(msg);
info_pub.publish(info);
ros::spinOnce();
loop_rate.sleep();
}
}
PathCollisionChecker()
{
ros::NodeHandle nh_;
dyn_rec_server_.setCallback(boost::bind(&PathCollisionChecker::dynRecParamCallback, this, _1, _2));
//grid_map_polygon_tools::setFootprintPoly(0.2, 0.2, this->footprint_poly_);
grid_map_polygon_tools::printPolyInfo(this->footprint_poly_);
//traversability_map_sub_ = nh_.subscribe("/dynamic_map", 2, &PathCollisionChecker::traversabilityMapCallback, this);
//safe_twist_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel_safe", 1, false);
ros::NodeHandle pnh("~");
marker_pub_ = pnh.advertise<visualization_msgs::MarkerArray>("path_collision_check_markers", 1,false);
debug_pose_ = pnh.advertise<geometry_msgs::PoseStamped>("/collision_pose",5, false);
pose_sub_ = pnh.subscribe("/robot_pose", 1, &PathCollisionChecker::poseCallback, this);
local_elevation_map_sub_ = pnh.subscribe("/elevation_mapping/elevation_map", 1, &PathCollisionChecker::localElevationMapCallback, this);
path_sub_ = pnh.subscribe("/smooth_path", 1, &PathCollisionChecker::pathCallback, this);
add_object_client_ = pnh.serviceClient<hector_worldmodel_msgs::AddObject>("/worldmodel/add_object", false);
pose_percept_publisher_ = pnh.advertise<hector_worldmodel_msgs::PosePercept>("/worldmodel/pose_percept", 5, false);
}
int main(int argc, char **argv){
ros::init(argc, argv, "cascade_classifier");
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
vision::CascadeClassifier classifier(nh, pnh);
ros::spin();
}
void RollPitchYawrateThrustControllerNode::InitializeParams() {
ros::NodeHandle pnh("~");
// Read parameters from rosparam.
GetRosParameter(pnh, "attitude_gain/x",
roll_pitch_yawrate_thrust_controller_.controller_parameters_.attitude_gain_.x(),
&roll_pitch_yawrate_thrust_controller_.controller_parameters_.attitude_gain_.x());
GetRosParameter(pnh, "attitude_gain/y",
roll_pitch_yawrate_thrust_controller_.controller_parameters_.attitude_gain_.y(),
&roll_pitch_yawrate_thrust_controller_.controller_parameters_.attitude_gain_.y());
GetRosParameter(pnh, "attitude_gain/z",
roll_pitch_yawrate_thrust_controller_.controller_parameters_.attitude_gain_.z(),
&roll_pitch_yawrate_thrust_controller_.controller_parameters_.attitude_gain_.z());
GetRosParameter(pnh, "angular_rate_gain/x",
roll_pitch_yawrate_thrust_controller_.controller_parameters_.angular_rate_gain_.x(),
&roll_pitch_yawrate_thrust_controller_.controller_parameters_.angular_rate_gain_.x());
GetRosParameter(pnh, "angular_rate_gain/y",
roll_pitch_yawrate_thrust_controller_.controller_parameters_.angular_rate_gain_.y(),
&roll_pitch_yawrate_thrust_controller_.controller_parameters_.angular_rate_gain_.y());
GetRosParameter(pnh, "angular_rate_gain/z",
roll_pitch_yawrate_thrust_controller_.controller_parameters_.angular_rate_gain_.z(),
&roll_pitch_yawrate_thrust_controller_.controller_parameters_.angular_rate_gain_.z());
GetVehicleParameters(pnh, &roll_pitch_yawrate_thrust_controller_.vehicle_parameters_);
roll_pitch_yawrate_thrust_controller_.InitializeParameters();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "gps_to_pose_conversion_node");
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
g_got_imu = false;
g_got_altitude = false;
// Use different coordinate transform if using simulator
if (!pnh.getParam("is_sim", g_is_sim)) {
ROS_WARN("Could not fetch 'sim' param, defaulting to 'false'");
g_is_sim = false;
}
// FIXME: if parameters not found and using defaults, throw a ROS_WARN
// Specify whether covariances should be set manually or from GPS
ros::param::param("~trust_gps", g_trust_gps, false);
// Get manual parameters
ros::param::param("~fixed_covariance/position/x", g_covariance_position_x, 4.0);
ros::param::param("~fixed_covariance/position/y", g_covariance_position_y, 4.0);
ros::param::param("~fixed_covariance/position/z", g_covariance_position_z, 100.0);
ros::param::param("~fixed_covariance/orientation/x", g_covariance_orientation_x, 0.02);
ros::param::param("~fixed_covariance/orientation/y", g_covariance_orientation_y, 0.02);
ros::param::param("~fixed_covariance/orientation/z", g_covariance_orientation_z, 0.11);
// Wait until GPS reference parameters are initialized.
double latitude, longitude, altitude;
do {
ROS_INFO("Waiting for GPS reference parameters...");
if (nh.getParam("/gps_ref_latitude", latitude) && nh.getParam("/gps_ref_longitude", longitude)
&& nh.getParam("/gps_ref_altitude", altitude)) {
g_geodetic_converter.initialiseReference(latitude, longitude, altitude);
} else {
ROS_INFO("GPS reference not ready yet, use set_gps_reference_node to set it");
ros::Duration(0.5).sleep(); // sleep for half a second
}
} while (!g_geodetic_converter.isInitialised());
// Show reference point
double initial_latitude, initial_longitude, initial_altitude;
g_geodetic_converter.getReference(&initial_latitude, &initial_longitude, &initial_altitude);
ROS_INFO("GPS reference initialized correctly %f, %f, %f", initial_latitude, initial_longitude,
initial_altitude);
// Initialize publishers
g_gps_pose_pub = nh.advertise < geometry_msgs::PoseWithCovarianceStamped > ("gps_pose", 1);
g_gps_transform_pub = nh.advertise < geometry_msgs::TransformStamped > ("gps_transform", 1);
g_gps_position_pub = nh.advertise < geometry_msgs::PointStamped > ("gps_position", 1);
// Subscribe to IMU and GPS fixes, and convert in GPS callback
ros::Subscriber imu_sub = nh.subscribe("imu", 1, &imu_callback);
ros::Subscriber gps_sub = nh.subscribe("gps", 1, &gps_callback);
ros::Subscriber altitude_sub = nh.subscribe("external_altitude", 1, &altitude_callback);
ros::spin();
}
DrivingAidMarker()
{
ros::NodeHandle pnh("~");
pnh.param("left_side_y_outer", p_left_side_y_outer_, 0.0);
pnh.param("left_side_y_inner", p_left_side_y_inner_, 0.0);
pnh.param("right_side_y_outer", p_right_side_y_outer_, 0.0);
pnh.param("right_side_y_inner", p_right_side_y_inner_, 0.0);
pnh.param("offset_z", p_offset_z_, -0.112-0.07);
pub_timer_ = pnh.createTimer(ros::Duration(0.1), &DrivingAidMarker::pubTimerCallback, this, false);
marker_pub_ = pnh.advertise<visualization_msgs::MarkerArray>("driving_aid", 1,false);
visualization_msgs::Marker marker;
//marker.header.stamp = req.point.header.stamp;
marker.header.frame_id = "/base_link";
marker.type = visualization_msgs::Marker::LINE_LIST;
marker.action = visualization_msgs::Marker::ADD;
marker.color.r= 1.0;
marker.color.a = 1.0;
marker.scale.x = 0.02;
marker.ns ="wheel_footprint";
marker.action = visualization_msgs::Marker::ADD;
marker.pose.orientation.w = 1.0;
std::vector<geometry_msgs::Point> point_vector;
geometry_msgs::Point tmp;
tmp.x = -1.5;
tmp.z = p_offset_z_;
tmp.y = p_left_side_y_outer_;
point_vector.push_back(tmp);
tmp.x = 1.5;
point_vector.push_back(tmp);
tmp.x = -1.5;
tmp.y = p_left_side_y_inner_;
point_vector.push_back(tmp);
tmp.x = 1.5;
point_vector.push_back(tmp);
tmp.x = -1.5;
tmp.y = p_right_side_y_outer_;
point_vector.push_back(tmp);
tmp.x = 1.5;
point_vector.push_back(tmp);
tmp.x = -1.5;
tmp.y = p_right_side_y_inner_;
point_vector.push_back(tmp);
tmp.x = 1.5;
point_vector.push_back(tmp);
marker.points = point_vector;
marker_array_.markers.push_back(marker);
}
int main(int argc, char *argv[]) {
ros::init(argc, argv, "health_analyzer");
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
HealthAnalyzer analyzer(nh, pnh);
ros::spin();
return 0;
}
HilSensorLevelInterface::HilSensorLevelInterface(const Eigen::Quaterniond& q_S_B) {
ros::NodeHandle pnh("~");
// Retrieve the necessary parameters.
double gps_freq;
std::string air_speed_sub_topic;
std::string gps_sub_topic;
std::string ground_speed_sub_topic;
std::string imu_sub_topic;
std::string mag_sub_topic;
std::string pressure_sub_topic;
pnh.param("gps_frequency", gps_freq, kDefaultGpsFrequency);
pnh.param("air_speed_topic", air_speed_sub_topic, std::string(mav_msgs::default_topics::AIR_SPEED));
pnh.param("gps_topic", gps_sub_topic, std::string(mav_msgs::default_topics::GPS));
pnh.param("ground_speed_topic", ground_speed_sub_topic, std::string(mav_msgs::default_topics::GROUND_SPEED));
pnh.param("imu_topic", imu_sub_topic, std::string(mav_msgs::default_topics::IMU));
pnh.param("mag_topic", mag_sub_topic, std::string(mav_msgs::default_topics::MAGNETIC_FIELD));
pnh.param("pressure_topic", pressure_sub_topic, kDefaultPressureSubTopic);
// Compute the desired interval between published GPS messages.
gps_interval_nsec_ = static_cast<uint64_t>(kSecToNsec / gps_freq);
// Compute the rotation matrix to rotate data into NED frame
q_S_B_ = q_S_B;
R_S_B_ = q_S_B_.matrix().cast<float>();
// Initialize the subscribers.
air_speed_sub_ =
nh_.subscribe<geometry_msgs::TwistStamped>(
air_speed_sub_topic, 1, boost::bind(
&HilListeners::AirSpeedCallback, &hil_listeners_, _1, &hil_data_));
gps_sub_ =
nh_.subscribe<sensor_msgs::NavSatFix>(
gps_sub_topic, 1, boost::bind(
&HilListeners::GpsCallback, &hil_listeners_, _1, &hil_data_));
ground_speed_sub_ =
nh_.subscribe<geometry_msgs::TwistStamped>(
ground_speed_sub_topic, 1, boost::bind(
&HilListeners::GroundSpeedCallback, &hil_listeners_, _1, &hil_data_));
imu_sub_ =
nh_.subscribe<sensor_msgs::Imu>(
imu_sub_topic, 1, boost::bind(
&HilListeners::ImuCallback, &hil_listeners_, _1, &hil_data_));
mag_sub_ =
nh_.subscribe<sensor_msgs::MagneticField>(
mag_sub_topic, 1, boost::bind(
&HilListeners::MagCallback, &hil_listeners_, _1, &hil_data_));
pressure_sub_ =
nh_.subscribe<sensor_msgs::FluidPressure>(
pressure_sub_topic, 1, boost::bind(
&HilListeners::PressureCallback, &hil_listeners_, _1, &hil_data_));
}
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);
}
Marker6DOF() {
ros::NodeHandle nh, pnh("~");
pnh.param("frame_id", frame_id_, std::string("/map"));
pose_pub_ = pnh.advertise<geometry_msgs::PoseStamped>("pose", 1);
pose_stamped_sub_ = pnh.subscribe("move_marker", 1, &Marker6DOF::moveMarkerCB, this);
server_.reset( new interactive_markers::InteractiveMarkerServer(ros::this_node::getName()));
initializeInteractiveMarker();
}
OdometryROS::~OdometryROS()
{
ros::NodeHandle pnh("~");
for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
{
pnh.deleteParam(iter->first);
}
delete odometry_;
}
OdomMsgToTF() :
frameId_(""),
odomFrameId_("")
{
ros::NodeHandle pnh("~");
pnh.param("frame_id", frameId_, frameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
ros::NodeHandle nh;
odomTopic_ = nh.subscribe("odom", 1, &OdomMsgToTF::odomReceivedCallback, this);
}
int main(int argc, char** argv) {
ros::init(argc, argv, "gps_odom");
ros::NodeHandle nh, pnh("~");
try {
gps_odom::Node node(nh, pnh);
ros::spin();
}
catch (const std::exception& e) {
ROS_ERROR("%s: %s", pnh.getNamespace().c_str(), e.what());
}
}
int main(int argc, char** argv)
{
pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
ros::init(argc, argv, "footstep_planner");
ros::NodeHandle pnh("~");
#if DEBUG
jsk_footstep_planner::pub_debug_marker = pnh.advertise<visualization_msgs::MarkerArray>("debug_marker_array", 1);
#endif
jsk_footstep_planner::FootstepPlanner planner(pnh);
ros::spin();
}
int main(int argc, char **argv) {
ros::init(argc, argv, "apriltag_detector_node");
ros::NodeHandle pnh("~");
try {
apriltag_ros::ApriltagDetectorNode node(pnh);
ros::spin();
} catch (const std::exception &e) {
ROS_ERROR("%s: %s", pnh.getNamespace().c_str(), e.what());
}
}
int main(int argc, char **argv){
ros::init(argc, argv, "astra_camera");
ros::NodeHandle n;
ros::NodeHandle pnh("~");
astra_wrapper::AstraDriver drv(n, pnh);
ros::spin();
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc,argv,"worldmodel_main");
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
vigir_worldmodel::WorldmodelCore wc(nh,pnh);
ros::spin();
return 0;
}
int main(int argc, char **argv){
ros::init(argc, argv, "openni2_camera");
ros::NodeHandle n;
ros::NodeHandle pnh("~");
head_time_calibration_tools::HeadTimeCalibration htc(n, pnh);
ros::spin();
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "motors");
ros::NodeHandle pnh("~");
motors::Node node(pnh);
ros::spin();
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "cloud_get_surface");
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
// the first string is the variable name used by roslaunch, followed by variable value defined by roslaunch, and the default value
pnh.param("max_plane_cloud_topic", pointCloudMaxPlaneTopic_, pointCloudMaxPlaneTopic_);
pnh.param("max_plane_pose_topic", poseMaxPlaneTopic_, poseMaxPlaneTopic_);
pnh.param("ground_cloud_topic", pointCloudGroundTopic_, pointCloudGroundTopic_);
pnh.param("ground_pose_topic", poseGroundTopic_, poseGroundTopic_);
pnh.param("camera_pitch_offset", OFFSET, OFFSET);
pnh.param("ground_to_base_height", GH, GH);
posePubPlaneMax_ = nh.advertise<geometry_msgs::PoseStamped>(poseMaxPlaneTopic_, 1);
cloudPubPlaneMax_ = nh.advertise<sensor_msgs::PointCloud2>(pointCloudMaxPlaneTopic_, 1);
posePubGround_ = nh.advertise<geometry_msgs::PoseStamped>(poseGroundTopic_, 1);
cloudPubGround_ = nh.advertise<sensor_msgs::PointCloud2>(pointCloudGroundTopic_, 1);
feedbackGetSurfaceType_ = nh.advertise<std_msgs::Int8>("status/get/surface/feedback", 1);
ros::Subscriber sub_roll = nh.subscribe<std_msgs::Float32>("/camera_pitch", 1, rollCallback);
ros::Subscriber sub = nh.subscribe<PointCloud>("/point_cloud", 1, cloudCallback);
while (ros::ok())
{
if (ros::param::has("camera_pitch_offset"))
ros::param::get("camera_pitch_offset", OFFSET);
if (ros::param::has("ground_to_base_height"))
ros::param::get("ground_to_base_height", GH);
if (ros::param::has(param_running_mode))
ros::param::get(param_running_mode, modeType_);
std_msgs::Int8 typeNum;
if (modeType_ == m_track)
{
ROS_INFO_THROTTLE(19, "Get surface is not activated.\n");
continue;
}
ros::spinOnce();
typeNum.data = fType_;
feedbackGetSurfaceType_.publish(typeNum);
}
return 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "bounding_box_interactive_marker");
ros::NodeHandle n, pnh("~");
server.reset(new interactive_markers::InteractiveMarkerServer("bounding_box_interactive_marker", "", false));
pub = pnh.advertise<jsk_pcl_ros::Int32Stamped>("selected_index", 1);
box_pub = pnh.advertise<jsk_pcl_ros::BoundingBox>("selected_box", 1);
box_arr_pub = pnh.advertise<jsk_pcl_ros::BoundingBoxArray>("selected_box_array", 1);
ros::Subscriber sub = pnh.subscribe("bounding_box_array", 1, boxCallback);
ros::spin();
server.reset();
return 0;
}
int main(int argc, char** argv) {
ros::init(argc, argv, "flea3_stereo");
ros::NodeHandle pnh("~");
try {
flea3::StereoNode stereo_node(pnh);
stereo_node.Run();
ros::spin();
stereo_node.End();
} catch (const std::exception& e) {
ROS_ERROR("%s: %s", pnh.getNamespace().c_str(), e.what());
}
}