订户没有输出

时间:2017-06-08 03:15:58

标签: c++ ros

这是订户代码的一部分。我想在回调中打印出ytmp,但是没有打印,甚至没有打印过。不知道我的代码在这里有什么问题。它确实在while内打印出ytmp(ros :: ok())。为什么跳过回调?

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include "std_msgs/String.h"
#include <geometry_msgs/Vector3.h>
double ytmp,ztmp;

void SubCallback(const geometry_msgs::Vector3::ConstPtr& msg)
{
   //ROS_INFO("I heard: [%f]", msg->y);
   //ROS_INFO("I heard: [%f]", msg->z);   
   ytmp = msg->y; 
   ztmp = msg->z;
   std::cout<<"ytmp CALLBACK = "<<ytmp<<std::endl;
}


int main( int argc, char** argv )
{


   ros::init(argc, argv, "basic_shapes");
   ros::NodeHandle n;
   ros::Rate r(1);
   ros::Subscriber sub = n.subscribe("intersect_values", 1000, SubCallback);
   ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);

   // Set our initial shape type to be a cube
   uint32_t shape = visualization_msgs::Marker::SPHERE;

   while (ros::ok())
   {
     std::cout<<"abc = "<<ytmp<<std::endl;
     visualization_msgs::Marker marker;
     // Set the frame ID and timestamp.  See the TF tutorials for information on these.
     marker.header.frame_id = "world";
     marker.header.stamp = ros::Time::now();

     // Set the namespace and id for this marker.  This serves to create a unique ID
     // Any marker sent with the same namespace and id will overwrite the old one
     marker.ns = "basic_shapes";
     marker.id = 0;

     // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
     // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
     marker.action = visualization_msgs::Marker::ADD;

     // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
     marker.pose.position.x = 0;// rwr_x;// joint1_x;        // 2.52782791;
     marker.pose.position.y = ytmp;//cord_y;// rwr_y;//joint1_y;        //0.0182385;

     std::cout<<"ytmp = "<<ytmp<<std::endl;         

     marker.pose.position.z = ztmp;//cord_z;// rwr_z;//joint1_z;        //0;
     marker.pose.orientation.x = 0.0;
     marker.pose.orientation.y = 0.0;
     marker.pose.orientation.z = 0.0;
     marker.pose.orientation.w = 1.0;

     // Set the scale of the marker -- 1x1x1 here means 1m on a side
     marker.scale.x = 0.2;
     marker.scale.y = 0.2;
     marker.scale.z = 0.2;

     // Set the color -- be sure to set alpha to something non-zero!
     marker.color.r = 1.0f;
     marker.color.g = 1.0f;
     marker.color.b = 0.0f;
     marker.color.a = 2.0;

     marker.lifetime = ros::Duration();

     // Publish the marker
    /* while (marker_pub.getNumSubscribers() < 1)
     {
    //std::cout << "marker_pub.getNumSubscribers() = " <<    marker_pub.getNumSubscribers() << std::endl;
       if (!ros::ok())
       {
     return 0;
       }
       ROS_WARN_ONCE("Please create a subscriber to the marker");
       //sleep(1);
       }*/
       marker_pub.publish(marker);
       r.sleep();
   }
}

0 个答案:

没有答案