Project

General

Profile

Revision 3bdc7bfd

ID3bdc7bfd383eff8b8dc04226581ac742720e33b6

Added by Thomas Mullins about 12 years ago

Changed rviz target to publish constantly

View differences:

vision/src/rviz_interactive_target.cpp
9 9 

  		




  10
  10
  
    
/* TODO:


  		




  11
  11
  
    
 * -replace the three MOVE_AXIS directions with one MOVE_AXIS and one MOVE_PLANE


  		




  12
  
  
    
 * -have it constantly publish current position, instead of only publishing


  		




  13
  
  
    
 *  when the marker is moved


  		




  14
  12
  
    
 */


  		




  15
  13
  
    

  		




  16
  14
  
    
class Marker3DOF


  		




  17
  15
  
    
{


  		




  18
  16
  
    
public:


  		




  19
  
  
    
  Marker3DOF(interactive_markers::InteractiveMarkerServer& server);


  		




  
  17
  
    
  Marker3DOF(interactive_markers::InteractiveMarkerServer& server, int pubRate);


  		




  20
  18
  
    
  void processFeedback(const InteractiveMarkerFeedbackConstPtr& feedback);


  		




  
  19
  
    
  void timerCallback(const ros::TimerEvent& event);


  		




  21
  20
  
    
private:


  		




  22
  21
  
    
  void addControl(string name, float x, float y, float z, float w);


  		




  23
  22
  
    
  ros::NodeHandle nh;


  		




  24
  
  
    
  InteractiveMarker marker;


  		




  25
  23
  
    
  ros::Publisher pub;


  		




  
  24
  
    
  ros::Timer timer;


  		




  
  25
  
    
  InteractiveMarker marker;


  		




  26
  26
  
    
};


  		




  27
  27
  
    

  		




  28
  
  
    
Marker3DOF::Marker3DOF(interactive_markers::InteractiveMarkerServer& server)


  		




  
  28
  
    
Marker3DOF::Marker3DOF(interactive_markers::InteractiveMarkerServer& server,


  		




  
  29
  
    
    int pubRate = 0)


  		




  29
  30
  
    
{


  		




  30
  31
  
    
  marker.header.frame_id = "/quadrotor";


  		




  31
  32
  
    
  marker.pose.position.x = 5;


  		




  ......




  40
  41
  
    
  server.setCallback("target", bind(&Marker3DOF::processFeedback, this, _1));


  		




  41
  42
  
    
  server.applyChanges();


  		




  42
  43
  
    
  pub = nh.advertise<geometry_msgs::Point>("/v2v3_converter/target_3d", 100);


  		




  
  44
  
    
  if (pubRate > 0)


  		




  
  45
  
    
    timer = nh.createTimer(ros::Duration(1./pubRate),


  		




  
  46
  
    
        bind(&Marker3DOF::timerCallback, this, _1));


  		




  43
  47
  
    
}


  		




  44
  48
  
    

  		




  45
  49
  
    
void Marker3DOF::addControl(string name, float x, float y, float z, float w)


  		




  ......




  54
  58
  
    
  marker.controls.push_back(control);


  		




  55
  59
  
    
}


  		




  56
  60
  
    

  		




  57
  
  
    
void Marker3DOF::processFeedback(const InteractiveMarkerFeedbackConstPtr&


  		




  58
  
  
    
    feedback)


  		




  
  61
  
    
void Marker3DOF::processFeedback(const InteractiveMarkerFeedbackConstPtr& feedback)


  		




  
  62
  
    
{


  		




  
  63
  
    
  marker.pose.position = feedback->pose.position;


  		




  
  64
  
    
}


  		




  
  65
  
    

  		




  
  66
  
    
void Marker3DOF::timerCallback(const ros::TimerEvent& event)


  		




  59
  67
  
    
{


  		




  60
  
  
    
  pub.publish(feedback->pose.position);


  		




  
  68
  
    
  pub.publish(marker.pose.position);


  		




  61
  69
  
    
}


  		




  62
  70
  
    

  		




  63
  71
  
    
int main(int argc, char **argv)


  		




  ......




  65
  73
  
    
  ros::init(argc, argv, "rviz_interactive_targets");


  		




  66
  74
  
    
  interactive_markers::InteractiveMarkerServer server(


  		




  67
  75
  
    
      "rviz_interactive_targets");


  		




  68
  
  
    
  Marker3DOF marker(server);


  		




  
  76
  
    
  Marker3DOF marker(server, 10);


  		




  69
  77
  
    
  ros::spin();


  		




  70
  78
  
    
  return 0;


  		




  71
  79
  
    
}


  		












Also available in:  Unified diff