ImageFeed::ImageFeed(std::string path) : it_(nh_)
{
pub_ = it_.advertise("gscam/image_raw", 1);
sub_ = nh_.subscribe("cmd_vel", 1, &ImageFeed::commandCallback, this);
msg_ptr_.reset (new cv_bridge::CvImage);
path_ = path;
cv::namedWindow(WINDOW,CV_WINDOW_AUTOSIZE);
x = 0;
y = 0;
angle = 0;
if(nh_.ok()) publishImage();
//Republish command if no command recieved after 10s
/*ros::Rate loop_rate(0.1);
while (nh_.ok()) {
pub_.publish(msg_ptr_->toImageMsg());
ros::spinOnce();
loop_rate.sleep();
}*/
}
bool nodeCameras::CamerasServerCallback(CameraServer::CameraServerMessage::Request &req,
CameraServer::CameraServerMessage::Response &res)
{
int EyeIndex;
wxString tempString = _T("Service called, message is ");
std::string stlstring = req.Command;
wxString Message(stlstring.c_str(), wxConvUTF8);
tempString << Message;
std::cout << std::string(tempString.mb_str()) << std::endl;
if (Message.IsSameAs(_T("Quit")))
{
m_AllDone = true;
return false;
}
if (Message.IsSameAs(_T("FrameGrab")))
{
EyeIndex = req.EyeIndex;
//m_image = cvLoadImage("/home/dbarry/Documents/ROS_projects/CameraNode/src/Hokuyo.jpg");
m_image = m_pCameras->FrameGrab(EyeIndex);
if (m_image)
{
// if (EyeIndex == FORWARD_CAMERA)
// {
//m_RotatedImage = m_pCameras->RotateNinetyDegrees(m_image); //RotateImage(m_image, 90); // the forward camera is rotated 90 degrees.
//cvShowImage("Forward Eye", m_RotatedImage);
// cvShowImage("Forward Eye", m_image);
// }
publishImage(EyeIndex);
// cvReleaseImage(&m_RotatedImage); // have to do this to prevent memory leak *****************************************************************************************
}
else
{
std::cout << "Failed to grab image" << std::endl;
return false;
}
return true;
}
return false; // comand not recognized
}
void FileSeqPubCore::process()
{
if (one_frame_ == -1) //play back set of frames
{
step_++;
}
else //play back only one chosen frame
{
step_ = one_frame_;
if (step_ > (int)temp_trajectory_.size())
{
ROS_ERROR("Chosen frame is not exist!");
ros::shutdown();
}
}
if (step_ < (int)temp_trajectory_.size()) //process step
{
ROS_INFO("---------------Processing step %d...---------------\n", step_);
acquirePose();
acquireImage();
publishOdom();
publishImage();
}
else //if end of loaded data
{
if (loop_f_) //if want to play from begginning
{
ROS_INFO("----Start from the beginning...----\n");
step_ = -1;
}
else //end of publishing
{
ROS_INFO("----Whole trajectory was published. Quitting...----\n");
ros::shutdown();
}
}
}
void ImageFeed::commandCallback(const geometry_msgs::Twist& cmd)
{
if (cmd.linear.x > 0) {
if (angle > -0.785398163 && angle < 0.785398163)
x+=1;
else if (angle > 2.35619449 || angle < -2.35619449)
x-=1;
else if (angle > 0.785398163 && angle < 2.35619449)
y+=1;
else if (angle > -2.35619449 && angle < -0.785398163)
y-=1;
}
angle += cmd.angular.z;
if (angle > 3.14)
angle -= 6.28;
else if (angle <= -3.14)
angle += 6.28;
publishImage();
ROS_INFO("received cmd, new pos x: %d, y: %d, angle: %f", x, y, angle);
}