42 #include <eigen3/Eigen/Core> 43 #include <eigen3/Eigen/Geometry> 100 bool take_snapshot =
false;
116 cv::Stitcher stitcher = cv::Stitcher::createDefault(
false);
117 cv::Stitcher::Status status = stitcher.stitch(
images_, pano);
118 log(
"Finished Stiching");
125 log(
"Publishing Completed Panorama");
138 log(
"Continuous Mode panorama");
150 take_snapshot =
true;
157 take_snapshot =
false;
161 std::stringstream ss;
163 ss <<
"Waiting for robot to stop ... (speed = " <<
ang_vel_cur <<
")";
209 static double heading_last = 0.0f;
210 double heading = 0.0f;
212 Eigen::AngleAxisf angle_axis(Eigen::Quaternionf(msg->pose.pose.orientation.w,
213 msg->pose.pose.orientation.x,
214 msg->pose.pose.orientation.y,
215 msg->pose.pose.orientation.z));
216 Eigen::Vector3f axis = angle_axis.axis();
220 heading = angle_axis.angle();
222 else if (axis(2) < 0.0)
224 heading = -1.0 * angle_axis.angle();
228 heading_last = heading;
236 turtlebot3_applications_msgs::TakePanorama::Response& response)
238 if (
is_active && (request.mode == request.CONTINUOUS || request.mode == request.SNAPANDROTATE))
240 log(
"Panorama creation already in progress.");
241 response.status = request.IN_PROGRESS;
243 else if (
is_active && (request.mode == request.STOP))
246 log(
"Panorama creation stopped.");
247 response.status = request.STOPPED;
250 else if (!
is_active && (request.mode == request.STOP))
252 log(
"No panorama creation in progress.");
253 response.status = request.STOPPED;
258 if (request.pano_angle <= 0.0)
260 log(
"Specified panorama angle is zero or negative! Panorama creation aborted.");
263 else if (request.snap_interval <= 0.0)
265 log(
"Specified snapshot interval is zero or negative! Panorama creation aborted.");
268 else if (request.rot_vel == 0.0)
270 log(
"Specified rotating speed is zero! Panorama creation aborted.");
277 cmd_vel.angular.z = request.rot_vel;
279 if (request.mode == turtlebot3_applications_msgs::TakePanoramaRequest::CONTINUOUS)
287 log(
"Starting panorama creation.");
290 response.status = request.STARTED;
299 std::cout <<
"encoding: " << msg->encoding << std::endl;
300 std::cout <<
"is_bigendian: " << msg->is_bigendian << std::endl;
310 ROS_ERROR(
"cv_bridge exception: %s", e.what());
314 images_.push_back(cv_ptr->image);
328 std_msgs::String msg;
334 int main(
int argc,
char **argv)
336 ros::init(argc, argv,
"turtlebot3_panorama");
339 pano.
log(
"Panorama app starting...");
341 pano.
log(
"Panorama application initialised.");
343 pano.
log(
"Bye, bye!");
geometry_msgs::Twist zero_cmd_vel
Subscriber subscribe(const std::string &base_topic, uint32_t queue_size, const boost::function< void(const sensor_msgs::ImageConstPtr &)> &callback, const ros::VoidPtr &tracked_object=ros::VoidPtr(), const TransportHints &transport_hints=TransportHints())
T radians_to_degrees(const T &radians)
ros::ServiceServer srv_start_pano
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void cameraImageCb(const sensor_msgs::ImageConstPtr &msg)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
image_transport::Publisher pub_stitched
bool takePanoServiceCb(turtlebot3_applications_msgs::TakePanorama::Request &request, turtlebot3_applications_msgs::TakePanorama::Response &response)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
ros::Publisher pub_cmd_vel
std::vector< cv::Mat > images_
image_transport::Subscriber sub_camera
void odomCb(const nav_msgs::OdometryConstPtr &msg)
void publish(const sensor_msgs::Image &message) const
int main(int argc, char **argv)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
geometry_msgs::Twist cmd_vel
void log(std::string msg)
T degrees_to_radians(const T °rees)
#define ROS_INFO_STREAM(args)
T wrap_angle(const T &angle)
#define ROS_INFO_STREAM_THROTTLE(rate, args)
ROSCPP_DECL void spinOnce()
sensor_msgs::ImagePtr toImageMsg() const