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(); }