← Back to branch summary

~ugocupcic/sr-ros-interface/manipulation

« back to all changes in this revision

Viewing changes to shadow_robot/dataglove/optical_synchronizer/src/camera_synchronizer.cpp

  • Committer: Ugo Cupcic
  • Date: 2011-05-23 15:44:05 UTC
  • mfrom: (119.1.67 sr-ros-interface)
  • Revision ID: ugo@shadowrobot.com-20110523154405-kwvv543sy9wxehzi
Huge merge from trunk. Lots of changes.

Show diffs side-by-side

added added

removed removed

Lines of Context:
shadow_robot/dataglove/optical_synchronizer/src/camera_synchronizer.cpp
1
 
#include <ros/ros.h>
2
 
#include "camera_synchronizer.h"
3
 
#include <tf/transform_broadcaster.h>
4
 
using namespace ros;
5
 
 
6
 
namespace camera_synchronizer{
7
 
 
8
 
  CameraSynchronizer::CameraSynchronizer()
9
 
        : n_tilde("~"), timestamp(0.0), publish_rate(0.0)
10
 
  {
11
 
  
12
 
  std::string searched_param;
13
 
  std::string path;
14
 
  
15
 
  //Set publish frequency for sync images
16
 
  double frequency;
17
 
 
18
 
  n_tilde.param("frequency",frequency,15.0);
19
 
  publish_rate = Rate(frequency);  
20
 
 
21
 
 
22
 
  //Set publishers and subscribers
23
 
  n_tilde.searchParam("left_async",searched_param);
24
 
  n_tilde.param(searched_param,path,std::string());
25
 
  
26
 
  left_sub = node.subscribe(path,10, &CameraSynchronizer::callback_left, this);
27
 
  
28
 
  n_tilde.searchParam("right_async",searched_param);
29
 
  n_tilde.param(searched_param,path,std::string());
30
 
 
31
 
  right_sub = node.subscribe(path,10, &CameraSynchronizer::callback_right, this);
32
 
  
33
 
  n_tilde.searchParam("left_info_async",searched_param);
34
 
  n_tilde.param(searched_param,path,std::string());
35
 
  
36
 
  left_info_sub = node.subscribe(path,10, &CameraSynchronizer::callback_info_left, this);
37
 
  
38
 
  n_tilde.searchParam("right_info_async",searched_param);
39
 
  n_tilde.param(searched_param,path,std::string());
40
 
 
41
 
  right_info_sub = node.subscribe(path,10, &CameraSynchronizer::callback_info_right, this);
42
 
 
43
 
  n_tilde.searchParam("left_sync",searched_param);
44
 
  n_tilde.param(searched_param,path,std::string());
45
 
  
46
 
  left_pub = node.advertise<sensor_msgs::Image>(path,1);
47
 
 
48
 
  n_tilde.searchParam("right_sync",searched_param);
49
 
  n_tilde.param(searched_param,path,std::string());
50
 
  
51
 
  right_pub = node.advertise<sensor_msgs::Image>(path,1);
52
 
 
53
 
  n_tilde.searchParam("left_info_sync",searched_param);
54
 
  n_tilde.param(searched_param,path,std::string());
55
 
  
56
 
  left_info_pub = node.advertise<sensor_msgs::CameraInfo>(path,1);
57
 
 
58
 
  n_tilde.searchParam("right_info_sync",searched_param);
59
 
  n_tilde.param(searched_param,path,std::string());
60
 
  
61
 
  right_info_pub = node.advertise<sensor_msgs::CameraInfo>(path,1);
62
 
  }
63
 
  
64
 
  void CameraSynchronizer::callback_left(const sensor_msgs::ImageConstPtr& msg){
65
 
  im_left=convertFromBoost(msg);
66
 
  static tf::TransformBroadcaster br;
67
 
  tf::Transform transform;
68
 
  transform.setOrigin(tf::Vector3(0.0,0.0,0.0));
69
 
  br.sendTransform(tf::StampedTransform(transform, ros::Time(timestamp),"fixed_frame", "test"));
70
 
  
71
 
  }
72
 
 
73
 
  void CameraSynchronizer::callback_right(const sensor_msgs::ImageConstPtr& msg){
74
 
 im_right=convertFromBoost(msg);
75
 
  }
76
 
 
77
 
void CameraSynchronizer::callback_info_left(const sensor_msgs::CameraInfoConstPtr& msg){
78
 
  info_left=convertFromBoost(msg);
79
 
//  info_left.roi.height=240;
80
 
//  info_left.roi.width=320;
81
 
  }
82
 
 
83
 
void CameraSynchronizer::callback_info_right(const sensor_msgs::CameraInfoConstPtr& msg){
84
 
  info_right=convertFromBoost(msg);
85
 
  int offset;
86
 
  n_tilde.param("right_offset", offset,0);
87
 
  info_right.roi.x_offset=offset;
88
 
//  info_right.roi.height=240;
89
 
//  info_right.roi.width=320;
90
 
  }
91
 
 
92
 
 
93
 
  sensor_msgs::Image CameraSynchronizer::convertFromBoost(const sensor_msgs::ImageConstPtr& msg){
94
 
        sensor_msgs::Image ret;
95
 
        ret.height=msg->height;
96
 
        ret.width=msg->width;
97
 
//      ret.header=msg->header;
98
 
        ret.header.stamp=ros::Time(timestamp);
99
 
//      ret.header.frame_id="/stereo/left/image_raw";
100
 
        ret.encoding="rgb8";
101
 
        ret.is_bigendian=msg->is_bigendian;
102
 
        ret.step=msg->step;
103
 
        ret.data=msg->data;
104
 
        return ret;
105
 
  }
106
 
 
107
 
  sensor_msgs::CameraInfo CameraSynchronizer::convertFromBoost(const sensor_msgs::CameraInfoConstPtr& msg){
108
 
        static tf::TransformBroadcaster br;
109
 
        tf::Transform transform;
110
 
        transform.setOrigin(tf::Vector3(0.0,0.0,0.0));
111
 
        br.sendTransform(tf::StampedTransform(transform, ros::Time(timestamp),"fixed_frame", "test"));
112
 
        sensor_msgs::CameraInfo ret;
113
 
        ret.height=msg->height;
114
 
        ret.width=msg->width;
115
 
        ret.roi=msg->roi;
116
 
        ret.D=msg->D;
117
 
        ret.K=msg->K;
118
 
        ret.R=msg->R;
119
 
        ret.P=msg->P;
120
 
        ret.header.stamp=ros::Time(timestamp);
121
 
        ret.header.frame_id="fixed_frame";
122
 
        return ret;
123
 
  }
124
 
 
125
 
 
126
 
  void CameraSynchronizer::publish(){
127
 
  left_pub.publish(im_left);
128
 
  right_pub.publish(im_right);
129
 
  left_info_pub.publish(info_left);
130
 
  right_info_pub.publish(info_right);
131
 
  timestamp+=1.0;
132
 
  ros::spinOnce();
133
 
  publish_rate.sleep();
134
 
  }
135
 
 
136
 
}