subsribe to a topic and publish at the same time

edit

I am using an Arduino to get ultrasonic distance.I publish the the distance value to the topic range_data .Now i have c++ code for publishing markers as given below.can i subscribe to range_data topic and publish markers based on that value.For simplicity i want the x location of the marker to be updated as new distance vaue from ultrasonic sensor.What changes need to be made in the following code

code

#include <ros/ros.h>
   #include <visualization_msgs/Marker.h>
   #include "sensor_msgs/Range.h"
#include "std_msgs/Bool.h"


   int main( int argc, char** argv )
   {
   l1:  ros::init(argc, argv, "basic_shapes");
     ros::NodeHandle n;
     ros::Rate r(1);
    ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);


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



     while (ros::ok())
     {

visualization_msgs::Marker marker;
       // Set the frame ID and timestamp.  See the TF tutorials for information on these.
       marker.header.frame_id = "/base_link";
       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
       marker.type = shape;

       // 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
 //ros::Subscriber sub = n.subscribe("range_data", 1000,rangeCallback);
       marker.pose.position.x =0; //need to be updated
       marker.pose.position.y = 0;
       marker.pose.position.z = 1;  
       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 = 2.0;
       marker.scale.y = 2.0;
       marker.scale.z = 2.0;

       // Set the color -- be sure to set alpha to something non-zero!
       marker.color.r = 0.20f;
       marker.color.g = 0.3f;
       marker.color.b = 0.2f;
       marker.color.a = 0.3;

       marker.lifetime = ros::Duration();

       // Publish the marker
       while (marker_pub.getNumSubscribers() < 1)
       {
         if (!ros::ok())
         {
           return 0;
         }
        // ROS_WARN_ONCE("Please create a subscriber to the marker");

sleep(1);
       }
       marker_pub.publish(marker);

       // Cycle between different shapes
       switch (3)
       {
       case visualization_msgs::Marker::CUBE:
         shape = visualization_msgs::Marker::SPHERE;
        break;
      case visualization_msgs::Marker::SPHERE:
        shape = visualization_msgs::Marker::ARROW;
        break;
      case visualization_msgs::Marker::ARROW:
        shape = visualization_msgs::Marker::CYLINDER;
        break;
      case visualization_msgs::Marker::CYLINDER:
       shape = visualization_msgs::Marker::CUBE;
       break;
      }

   r.sleep();

   }
  }