TargetFilter::TargetFilter()
{
// temporary variable
double window_size_in, pub_rate;
// load parameters
ros::NodeHandle nh_priv("~");
nh_priv.param<std::string>("parent_frame", parent_frame, "/base_link");
nh_priv.param<std::string>("target_frame", target_frame, "/target");
nh_priv.param<std::string>("target_filtered_frame", target_filtered_frame, "/target_filtered");
nh_priv.param("window_size", window_size_in, 1.0); // s
nh_priv.param("pub_rate", pub_rate, 10.0); // Hz
// lets show em what we got
ROS_INFO_STREAM("param parent_frame: " << parent_frame);
ROS_INFO_STREAM("param target_frame: " << target_frame);
ROS_INFO_STREAM("param target_filtered_frame: " << target_filtered_frame);
ROS_INFO_STREAM("param window_size: " << window_size_in);
ROS_INFO_STREAM("param pub_rate: " << pub_rate);
// setup
last_pub_time = ros::Time::now();
window_size = ros::Duration(window_size_in);
pub_period = ros::Duration(1.0 / pub_rate);
}
int main(int argc, char **argv) // ノードのメイン関数
{
ros::init(argc, argv, "ros_tutorial_srv_server"); // ノード名の初期化
ros::NodeHandle nh; // ROSシステムとの通信のためのノードのハンドルを宣言
ros::NodeHandle nh_priv("~"); // ROSシステムとの通信のためのノードのハンドルを宣言(Private)
// http://wiki.ros.org/Names
nh_priv.setParam("calculation_method", PLUS); // パラメータの初期設定, http://wiki.ros.org/rosparam
// サービスサーバを宣言し、 oroca_ros_tutorialsパッケージのsrvTutorialサービスファイルを利用した
// サービスサーバros_tutorial_service_serverを作成する。サービス名は「 ros_tutorial_srv 」であり、
// サービスの要求がある場合には、 calculationという名前の関数を実行する設定
ros::ServiceServer ros_tutorial_service_server = nh.advertiseService("ros_tutorial_srv", calculation);
ROS_INFO("ready srv server!");
ros::Rate r(10); // 10 hz
while (ros::ok())
{
nh_priv.getParam("calculation_method", g_operator); // 演算子をパラメータから取得した値に変更する
ros::spinOnce(); // コールバック関数の処理ルーチン
r.sleep(); // ルーチンを繰り返すためのsleep処理
}
return 0;
}
void erodePC(const PCRGB::Ptr& target_pc, const vector<int>& inds, PCRGB::Ptr& eroded_pc)
{
cv::Mat pc_img = cv::Mat::zeros(640, 480, CV_32F);
cv::Mat pc_img_eroded = cv::Mat::zeros(640, 480, CV_32F);
for(uint32_t i=0;i<inds.size();i++)
if(target_pc->points[i].x == target_pc->points[i].x &&
target_pc->points[i].y == target_pc->points[i].y &&
target_pc->points[i].z == target_pc->points[i].z) {
pc_img.at<uint8_t>(inds[i] % 640, inds[i] / 640) = 1.0;
}
IplImage pc_img_ipl = pc_img, pc_img_eroded_ipl = pc_img_eroded;
int num_erode;
ros::NodeHandle nh_priv("~");
nh_priv.param<int>("num_erode", num_erode, 2);
if(num_erode > 0)
cvErode(&pc_img_ipl, &pc_img_eroded_ipl, NULL, num_erode);
else
pc_img_eroded_ipl = pc_img_ipl;
pcl::IndicesPtr inds_erode(new vector<int>());
for(uint32_t i=0;i<inds.size();i++)
if(pc_img_eroded.at<uint8_t>(inds[i] % 640, inds[i] / 640) == 1.0)
inds_erode->push_back(i);
pcl::ExtractIndices<PRGB> extract_inds;
extract_inds.setInputCloud(target_pc);
extract_inds.setIndices(inds_erode);
extract_inds.filter(*eroded_pc);
}
// Check the flag for whether to draw the window
bool SystemFrontendBase::IsHeadless()
{
ros::NodeHandle nh_priv("~");
bool bHeadless;
nh_priv.param<bool>("headless", bHeadless, false);
return bHeadless;
}
TeleopXbox::TeleopXbox():
previous_enable_button_state(0),
previous_disable_button_state(0)
{
// load parameters
ros::NodeHandle nh_priv("~");
nh_priv.param("linear_x_scale", linear_x_scale, 0.1); // scaling from integer 10 (1g in m/s/s); 0.1 -> 1m/s
nh_priv.param("linear_y_scale", linear_y_scale, 0.8);
nh_priv.param("angular_z_scale", angular_z_scale, 0.2);
nh_priv.param("preboost_scale", preboost_scale, 1.0);
nh_priv.param("force_pub_rate", force_pub_rate, 10.0); // Hz
nh_priv.param("linear_x_axis", linear_x_axis, -1);
nh_priv.param("linear_y_axis", linear_y_axis, -1);
nh_priv.param("angular_z_axis", angular_z_axis, -1);
nh_priv.param("linear_y_left_button", linear_y_left_button, -1);
nh_priv.param("linear_y_right_button", linear_y_right_button, -1);
nh_priv.param("boost_button", boost_button, -1);
nh_priv.param("enable_button", enable_button, -1);
nh_priv.param("disable_button", disable_button, -1);
// lets show em what we got
ROS_INFO_STREAM("param linear_x_scale: " << linear_x_scale);
ROS_INFO_STREAM("param linear_y_scale: " << linear_y_scale);
ROS_INFO_STREAM("param angular_z_scale: " << angular_z_scale);
ROS_INFO_STREAM("param preboost_scale: " << preboost_scale);
ROS_INFO_STREAM("param force_pub_rate: " << force_pub_rate);
ROS_INFO_STREAM("param linear_x_axis: " << linear_x_axis);
ROS_INFO_STREAM("param linear_y_axis: " << linear_y_axis);
ROS_INFO_STREAM("param angular_z_axis: " << angular_z_axis);
ROS_INFO_STREAM("param linear_y_left_button: " << linear_y_left_button);
ROS_INFO_STREAM("param linear_y_right_button: " << linear_y_right_button);
ROS_INFO_STREAM("param boost_button: " << boost_button);
ROS_INFO_STREAM("param enable_button: " << enable_button);
ROS_INFO_STREAM("param disable_button: " << disable_button);
// connects subs and pubs
vel_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
joy_sub = nh.subscribe<sensor_msgs::Joy>("joy", 1, &TeleopXbox::joyCallback, this);
hazards_cli = nh.serviceClient<mecanumbot::RobotHazardsEnable>("robot_hazards_enable");
// always enable hazards if no enable button is set
if(enable_button < 0) {
callHazardsEnable(true);
}
// intial message that might be pushed out by the force_pub_rate
msg.linear.x = 0; // m/s
msg.linear.y = 0; // m/s
msg.angular.z = 0; // rad/s
// setup
last_pub_time = ros::Time::now();
force_pub_period = ros::Duration(1.0 / force_pub_rate);
}
Homography()
: nh_()
, it_(nh_)
, feature_detector_("SURF")
, descriptor_extractor_("SURF")
, descriptor_matcher_("FlannBased")
, matcher_filter_("CrossCheckFilter")
, threshold_(1.0)
{
ros::NodeHandle nh_priv("~");
nh_priv.param("detector", feature_detector_, feature_detector_);
nh_priv.param("matcher", descriptor_matcher_, descriptor_matcher_);
nh_priv.param("filter", matcher_filter_, matcher_filter_);
nh_priv.param("threshold", threshold_, threshold_);
detector_ = cv::FeatureDetector::create(feature_detector_);
if (detector_ == NULL)
{
ROS_FATAL_STREAM(
"Feature detector " << feature_detector_ <<
" not available!");
}
extractor_ = cv::DescriptorExtractor::create(descriptor_extractor_);
if (extractor_ == NULL)
{
ROS_FATAL_STREAM(
"Descriptor extractor " << descriptor_extractor_ <<
" not available!");
}
matcher_ = cv::DescriptorMatcher::create(descriptor_matcher_);
if (matcher_ == NULL)
{
ROS_FATAL_STREAM(
"Descriptor matcher " << descriptor_matcher_ <<
" not available!");
}
filter_ = getMatcherFilterType(matcher_filter_);
if (filter_ == -1)
{
ROS_FATAL_STREAM(
"Matcher filter " << matcher_filter_ <<
" not available!");
}
cv::namedWindow("correspondences", cv::WINDOW_NORMAL);
cv::namedWindow("homography", cv::WINDOW_NORMAL);
pub_ = it_.advertise("homography", 1);
sub_ = it_.subscribe("camera/image_raw", 1, &Homography::callback, this);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "velodyne_laserscan_node");
ros::NodeHandle nh;
ros::NodeHandle nh_priv("~");
// create VelodyneLaserScan class
velodyne_laserscan::VelodyneLaserScan n(nh, nh_priv);
// handle callbacks until shut down
ros::spin();
return 0;
}
/*!
* \brief Main Function
*
* \author Scott K Logan
*
* Initializes ROS, instantiates the node handle for the driver to use and
* instantiates the base_controller class.
*
* \param argc Number of command line arguments
* \param argv 2D character array of command line arguments
*
* \returns EXIT_SUCCESS, or an error state
*/
int main( int argc, char *argv[] )
{
ros::init( argc, argv, "base_controller" );
ros::NodeHandle nh;
ros::NodeHandle nh_priv( "~" );
base_controller::base_controller controller( nh, nh_priv );
controller.start( );
ros::spin( );
std::exit( EXIT_SUCCESS );
}
int main (int argc, char **argv)
{
ros::init(argc, argv, "joy_to_twist");
ros::NodeHandle nh;
ros::NodeHandle nh_priv( "~" );
joy_to_twist::joy_to_twist joy_to_twist( nh, nh_priv );
joy_to_twist.start();
ros::spin();
return 0;
}
/*!
* \brief Main Function
*
* \author Scott K Logan
*
* Initializes ROS, instantiates the node handle for the driver to use and
* instantiates the angel_controller class.
*
* \param argc Number of command line arguments
* \param argv 2D character array of command line arguments
*
* \returns EXIT_SUCCESS, or an error state
*/
int main( int argc, char *argv[] )
{
ros::init( argc, argv, "basic_twist_integrator" );
ros::NodeHandle nh;
ros::NodeHandle nh_priv( "~" );
basic_twist_integrator::basic_twist_integrator odom( nh, nh_priv );
odom.start( );
ros::spin( );
std::exit( EXIT_SUCCESS );
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "stereo_node");
ros::NodeHandle nh;
ros::NodeHandle nh_priv("~");
// Setup dynamic_reconfigure for runtime settings.
stereo_webcam::WebcamNode node(nh, nh_priv);
node.onInit();
while (ros::ok()) {
node.SpinOnce();
ros::spinOnce();
}
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "ellipse_detection_node");
ros::NodeHandle nh, nh_priv("~");
tf_listener_ = new tf::TransformListener();
// Reading params
bool params_loaded = true;
params_loaded *= nh_priv.getParam("img_in",img_in_topic_);
params_loaded *= nh_priv.getParam("contour_min_pix_size",contour_min_pix_size_);
params_loaded *= nh_priv.getParam("ellipse_max_ratio",ellipse_max_ratio_);
params_loaded *= nh_priv.getParam("hole_radius",hole_radius_);
params_loaded *= nh_priv.getParam("holes_min_spacing",holes_min_spacing_);
params_loaded *= nh_priv.getParam("debug",debug_);
params_loaded *= nh_priv.getParam("base_frame",base_frame_);
params_loaded *= nh_priv.getParam("out_pose_topic",out_pose_topic_);
params_loaded *= nh_priv.getParam("out_error_topic",out_error_topic_);
params_loaded *= nh_priv.getParam("fit_ellipse_max_error",fit_ellipse_max_error_);
if(!params_loaded){
ROS_ERROR("Couldn't find all the required parameters. Closing...");
return -1;
}
// ROS suscribers and publishers
image_transport::ImageTransport image_transport_(nh);
image_transport::CameraSubscriber depth_sub_ = image_transport_.subscribeCamera(img_in_topic_, 1, callback);
// pose0_pub_ = nh.advertise<geometry_msgs::PoseStamped>(out_pose_topic_+"_0", 1);
// pose1_pub_ = nh.advertise<geometry_msgs::PoseStamped>(out_pose_topic_+"_1", 1);
error_pub_ = nh_priv.advertise<geometry_msgs::Twist>(out_error_topic_, 1);
// OpenCV windows
if(debug_){
cv::namedWindow("Original Image");
cv::namedWindow("1/MedianBlur");
cv::namedWindow("2/MonoImage");
cv::namedWindow("3/AdaptiveThresholding");
cv::namedWindow("4/Opening");
}
cv::namedWindow("Ellipse detection");
ros::spin();
ros::shutdown();
return 0;
}
Pilot::Pilot() {
// load parameters
ros::NodeHandle nh_priv("~");
nh_priv.param("n_thrusters", n_thrusters, 8); // amount
nh_priv.param("pub_rate", pub_rate, 10.0); // Hz
// lets show em what we got
ROS_INFO_STREAM("param n_thrusters: " << n_thrusters);
ROS_INFO_STREAM("param pub_rate: " << pub_rate);
// connects subs and pubs
actuator_pub = nh.advertise<mavros_msgs::ActuatorControl>("actuator_control", 1);
cmd_vel_sub = nh.subscribe<geometry_msgs::Twist>("cmd_vel", 1, &Pilot::velCallback, this);
// setup
last_actuator_pub = ros::Time::now();
min_actuator_pub_period = ros::Duration(1.0 / pub_rate);
}
/*!
* \brief Main Function
*
* \author Scott K Logan
*
* Initializes ROS, instantiates the node handle for the driver to use and
* instantiates the PIKSI class.
*
* \param argc Number of command line arguments
* \param argv 2D character array of command line arguments
*
* \returns EXIT_SUCCESS, or an error state
*/
int main( int argc, char *argv[] )
{
ros::init( argc, argv, "piksi_node" );
ros::NodeHandle nh;
ros::NodeHandle nh_priv( "~" );
std::string port;
nh_priv.param( "port", port, (const std::string)"/dev/ttyUSB0" );
swiftnav_piksi::PIKSI piksi( nh, nh_priv, port );
ROS_DEBUG( "Opening Piksi on %s", port.c_str( ) );
if( !piksi.PIKSIOpen( ) )
ROS_ERROR( "Failed to open Piksi on %s", port.c_str( ) );
else
ROS_INFO( "Piksi opened successfully on %s", port.c_str( ) );
ros::spin( );
std::exit( EXIT_SUCCESS );
}
int main(int argc, char ** argv)
{
// initialize ROS and the node
ros::init(argc, argv, "robot_pose_publisher");
ros::NodeHandle nh;
ros::NodeHandle nh_priv("~");
// configuring parameters
std::string map_frame, base_frame;
double publish_frequency;
bool is_stamped;
ros::Publisher p_pub;
nh_priv.param<std::string>("map_frame",map_frame,"/map");
nh_priv.param<std::string>("footprint_frame",base_frame,"/base_footprint");
nh_priv.param<double>("publish_frequency",publish_frequency,10);
nh_priv.param<bool>("is_stamped", is_stamped, false);
if(is_stamped)
p_pub = nh.advertise<geometry_msgs::PoseStamped>("robot_pose", 1);
else
p_pub = nh.advertise<geometry_msgs::Pose>("robot_pose", 1);
// create the listener
tf::TransformListener listener;
listener.waitForTransform(map_frame, base_frame, ros::Time(), ros::Duration(1.0));
ros::Rate rate(publish_frequency);
while (nh.ok())
{
tf::StampedTransform transform;
try
{
listener.lookupTransform(map_frame, base_frame, ros::Time(0), transform);
// construct a pose message
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.frame_id = map_frame;
pose_stamped.header.stamp = ros::Time::now();
pose_stamped.pose.orientation.x = transform.getRotation().getX();
pose_stamped.pose.orientation.y = transform.getRotation().getY();
pose_stamped.pose.orientation.z = transform.getRotation().getZ();
pose_stamped.pose.orientation.w = transform.getRotation().getW();
pose_stamped.pose.position.x = transform.getOrigin().getX();
pose_stamped.pose.position.y = transform.getOrigin().getY();
pose_stamped.pose.position.z = transform.getOrigin().getZ();
if(is_stamped)
p_pub.publish(pose_stamped);
else
p_pub.publish(pose_stamped.pose);
}
catch (tf::TransformException &ex)
{
// just continue on
}
rate.sleep();
}
return EXIT_SUCCESS;
}
int main(int argc, char** argv){
ros::init(argc, argv, "pcl_viewer");
ros::NodeHandle nh, nh_priv("~");
ROS_INFO("Initializing viewer...");
// Initialize PointClouds
kinects_pc_ = boost::shared_ptr<PointCloudSM>(new PointCloudSM);
cluster_cloud_ = boost::shared_ptr<PointCloudSM>(new PointCloudSM);
normals_ = boost::shared_ptr<pcl::PointCloud<NormalType> >(new pcl::PointCloud<NormalType>);
robot_normals_ = boost::shared_ptr<pcl::PointCloud<NormalType> >(new pcl::PointCloud<NormalType>);
robot_pc_ = boost::shared_ptr<PointCloudSM>(new PointCloudSM);
nh_priv.getParam("rf_rad",rf_rad);
// Reserve memory for clouds
kinects_pc_->reserve(10000);
cluster_cloud_->reserve(10000);
normals_->reserve(10000);
robot_normals_->reserve(10000);
// Ros Subscribers and Publishers
ros::Subscriber kinect_pc_sub = nh.subscribe<PCMsg>("/kinectV2/qhd/points", 1, callback);
cluster_pc_pub_ = nh.advertise<PCMsg>("/kinectV2/qhd/test_points",1);
// ros::Subscriber kinect_pc_sub = nh.subscribe<PCMsg>("/kinect1/depth_registered/points", 1, callback);
// cluster_pc_pub_ = nh.advertise<PCMsg>("/kinect1/depth_registered/points_downsampled",1);
// ros::Subscriber kinect_pc_sub = nh.subscribe<PCMsg>("/robot_model_to_pointcloud/robot_cloud2", 1, callback);
// cluster_pc_pub_ = nh.advertise<PCMsg>("/robot_model_to_pointcloud/robot_cloud2_downsampled",1);
// Global params init
first_frame_ = true;
// Load robot model
robot_pcd_path_ = ros::package::getPath("kinect_pcl_obj_detection") + "/data/robot_0.005.pcd";
pcl::io::loadPCDFile(robot_pcd_path_,*robot_pc_);
// Shitf robot model
pclTransform pcl_shift;
tf::StampedTransform tf_shift;
tf::Vector3 vec3_shitf(0.0, 0.5, 0.0);
tf_shift.setOrigin(vec3_shitf);
tf::vectorTFToEigen(tf_shift.getOrigin(), pcl_shift.translation);
tf::quaternionTFToEigen(tf_shift.getRotation(), pcl_shift.rotation);
pcl::transformPointCloud(*robot_pc_, *robot_pc_, pcl_shift.translation, pcl_shift.rotation);
// Robot normals
norm_est_.setKSearch(10);
norm_est_.setInputCloud (robot_pc_);
norm_est_.compute (*robot_normals_);
viewer_robot_.addPointCloud(robot_pc_, "robot");
viewer_robot_.addPointCloudNormals<PointType, NormalType>(robot_pc_, robot_normals_, 1, 0.05, "robot_normals");
// PCL viewer
viewer_.setBackgroundColor (0.0, 0.0, 0.0);
viewer_.addPointCloud(kinects_pc_,"sample_cloud");
viewer_.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample_cloud");
viewer_.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 0.0, 1.0, "sample_cloud");
viewer_.initCameraParameters ();
ros::spin();
return 0;
}
int main(int argc, char **argv)
{
#ifdef USE_GLUT_RENDERING
glutInit(&argc, argv);
#else
const char *displayName = NULL;
glx_display = XOpenDisplay(displayName);
if (!glx_display)
{
ROS_ERROR_STREAM("No glx display!");
return 1;
}
static int visual_attribs[] = {
GLX_X_RENDERABLE , True,
GLX_DRAWABLE_TYPE , GLX_WINDOW_BIT,
GLX_RENDER_TYPE , GLX_RGBA_BIT,
GLX_X_VISUAL_TYPE , GLX_TRUE_COLOR,
GLX_RED_SIZE , 8,
GLX_GREEN_SIZE , 8,
GLX_BLUE_SIZE , 8,
GLX_ALPHA_SIZE , 8,
GLX_DEPTH_SIZE , 24,
GLX_STENCIL_SIZE , 8,
GLX_DOUBLEBUFFER , True,
None
};
int num_fb_configs = 0;
GLXFBConfig* fbConfigs = glXChooseFBConfig(
glx_display, DefaultScreen(glx_display), visual_attribs, &num_fb_configs);
if (!num_fb_configs)
{
ROS_ERROR_STREAM("No fb configs!");
return 1;
}
else
{
ROS_DEBUG_STREAM("Found " << num_fb_configs << " configs");
}
#endif // USE_GLUT_RENDERING
ros::init(argc, argv, "robot_self_filter");
//create selfilter service
ros::NodeHandle nh;
ros::NodeHandle nh_priv("~");
//set parameters
bool publish_alpha_image;
std::string camera_topic;
std::string camera_info_topic;
nh_priv.param("publish_mask",publish_mask, true);
nh_priv.param("publish_alpha",publish_alpha_image, true);
nh_priv.param("inverted",inverted, false);
nh_priv.param<std::string>("camera_topic", camera_topic, "/wide_stereo/right/image_rect_color");
nh_priv.param<std::string>("camera_info_topic", camera_info_topic, "/wide_stereo/right/camera_info");
ROS_INFO("camera topic %s", camera_topic.c_str());
ROS_INFO("camera info %s", camera_info_topic.c_str());
//create robot model
ROS_DEBUG_STREAM("Getting robmod");
robmod = new RobotMeshModel;
ROS_DEBUG_STREAM("Got robmod");
//initialize glut
#ifdef USE_GLUT_RENDERING
glutInitWindowSize (robmod->cam_info_->width, robmod->cam_info_->height);
glutInitDisplayMode (GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
glutCreateWindow ("dummy");
glutHideWindow();
#else // USE_GLUT_RENDERING
ROS_INFO_STREAM("Creating glX pbuffer");
int pbufferAttribs[] = {
GLX_PBUFFER_WIDTH , robmod->cam_info_->width,
GLX_PBUFFER_HEIGHT, robmod->cam_info_->height,
None
};
pbuffer = glXCreatePbuffer(glx_display, fbConfigs[0], pbufferAttribs);
if (!pbuffer)
{
ROS_ERROR_STREAM("Failed to create pbuffer!");
return 1;
}
glXCreateContextAttribsARBProc glXCreateContextAttribsARB = 0;
glXCreateContextAttribsARB = (glXCreateContextAttribsARBProc)
glXGetProcAddressARB((const GLubyte *) "glXCreateContextAttribsARB");
GLXContext gl_context = 0;
int context_attribs[] = {
GLX_CONTEXT_MAJOR_VERSION_ARB, 3,
GLX_CONTEXT_MINOR_VERSION_ARB, 0,
None
};
ROS_DEBUG_STREAM("Creating OpenGL context");
gl_context = glXCreateContextAttribsARB(glx_display, fbConfigs[0], 0,
True, context_attribs);
// Sync to ensure any errors generated are processed.
XSync(glx_display, False);
if (!gl_context)
{
ROS_ERROR_STREAM("Failed to create GL 3.0 context");
return -1;
}
ROS_DEBUG_STREAM("Got openGl context");
// clean up
XFree(fbConfigs);
XSync(glx_display, False);
// Make context current
glXMakeCurrent(glx_display, pbuffer, gl_context);
XSync(glx_display, False);
#endif // USE_GLUT_RENDERING
ROS_DEBUG_STREAM("Initializng GL");
initializeGL();
ROS_DEBUG_STREAM("Initialized GL");
#ifdef USE_GLUT_RENDERING
// Register glut callbacks:
glutDisplayFunc (displayFunc);
#endif //USE_GLUT_RENDERING
ipl_maskBGRA = cvCreateImage(cvSize(robmod->cam_info_->width,
robmod->cam_info_->height), IPL_DEPTH_8U, 4);
ipl_maskBW = cvCreateImage(cvSize(robmod->cam_info_->width,
robmod->cam_info_->height), IPL_DEPTH_8U, 1);
image_transport::ImageTransport it(nh);
if (publish_mask)
mask_publisher = it.advertise(camera_topic + "/self_mask", 10);
if (publish_alpha_image)
image_publisher = it.advertise(camera_topic +
"/image_rect_color_alpha_masked", 10);
image_transport::Subscriber image_sub;
ros::Subscriber camerea_info_sub;
if (publish_alpha_image)
{
image_sub = it.subscribe(camera_topic, 10, alpha_image_cb);
}
else
{
// Change to listen to camera topic to time stamps because gazebo
// simulation doesn't update the camera info topic time stamp
//camerea_info_sub = nh.subscribe(camera_info_topic, 10, mask_cb);
camerea_info_sub = nh.subscribe(camera_topic, 10, test_mask_cb);
}
ros::spin();
//clean up
cvReleaseImage(&ipl_maskBGRA);
cvReleaseImage(&ipl_maskBW);
delete robmod;
#ifndef USE_GLUT_RENDERING
ROS_DEBUG_STREAM("Cleaning up GLX");
glXMakeCurrent(glx_display, None, NULL);
glXDestroyPbuffer(glx_display, pbuffer);
glXDestroyContext(glx_display, gl_context);
XCloseDisplay(glx_display);
#endif // USE_GLUT_RENDERING
return 0;
}
