I trying to understand why my subscriber isn't outputting anything.. It seems like, it doesn't get called, but it doesn't make sense why it doesn't...
Anyone who could explain??
#include "image_converter.h"
ImageConverter::ImageConverter()
: it_(nh_),pos(2), vel(2)
{
// Subscrive to input video feed and publish output video feed
image_sub_ = it_.subscribe("/bumblebeePTU/left/image_rect_color", 1,
&ImageConverter::imageCb, this);
image_pub_ = it_.advertise("/image_converter/output_video", 1);
//position_prev_pan = 0;
//cv::namedWindow(OPENCV_WINDOW);
}
ImageConverter::~ImageConverter()
{
//cv::destroyWindow(OPENCV_WINDOW);
}
void ImageConverter::chatterCallback(const sensor_msgs::JointState::ConstPtr& msg)
{
position_prev_p = sin(msg->position[0]*0.000727220522);
position_prev_t = sin(msg->position[0]*0.000727220522);
ROS_INFO_STREAM("LALALALALALALALALALALALALALALALAL");
}
void ImageConverter::imageCb(const sensor_msgs::ImageConstPtr& msg)
{
ros::Publisher facedetect_pub = nh_.advertise("/ptu/cmd", 1);
ros::Subscriber sub = nh_.subscribe("/joint_states",1,&ImageConverter::chatterCallback,this);
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
sensor_msgs::JointState ms;
pair displacement = detectAndDisplay(cv_ptr->image);
double positions[9] = {0.6,0,-0.6,0.6,0,-0.6,0.6,0,-0.6};
// Detect and outputs next position to PTU
for(int i = 0; i<9; ++i){
pos[0] = positions[i];//ceil(sin(displacement.first*0.000727220522)*1000)/1000 + position_prev_p;
pos[1] = 0;//ceil(sin(displacement.second*0.000727220522)*1000)/1000 + position_prev_t;
ms.position = pos;
vel[0] = 0.3;//abs(pos[0]);//2*atan(displacement.first+position_prev[0]/2*physical_width)*0.01 ;
vel[1] = 0.3;//abs(pos[1]);
ms.velocity = vel;
ROS_INFO_STREAM("p: "<toImageMsg());
}
}
↧