Hi Sir,
Thank you for your help first.
And I done it just now, but I get error even commenting all of ROS_INFO and ROS_ERROR functions.
#include "geometry_msgs/Quaternion.h"
#include "nav_msgs/GetMap.h"
#include "nav_msgs/OccupancyGrid.h"
#include "nav_msgs/Odometry.h"
#include "ros/console.h"
#include "ros/ros.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/transform_listener.h"
#include <cstdio>
// #include "message_filters/subscriber.h"
// #include "message_filters/synchronizer.h"
// #include "message_filters/sync_policies/approximate_time.h"
using namespace std;
/**
* @brief Map generation node.
*/
class MapGenerator {
public:
std::string mapname_;
ros::Subscriber map_sub_;
ros::Subscriber imu_sub_;
bool saved_map_;
int threshold_occupied_;
int threshold_free_;
MapGenerator(const std::string &mapname, int threshold_occupied,
int threshold_free)
: mapname_(mapname), saved_map_(false),
threshold_occupied_(threshold_occupied),
threshold_free_(threshold_free) {
ros::NodeHandle n;
// ROS_INFO("Waiting for the map");
imu_sub_ = n.subscribe<nav_msgs::Odometry>("/odom", 1000, this);
map_sub_ = n.subscribe<nav_msgs::OccupancyGrid>(
"/map", 1000, boost::bind(&MapGenerator::mapCallback, this, _1, _2));
// message_filters::Subscriber<nav_msgs::OccupancyGrid> map_sub(n, "map",
// 1); message_filters::Subscriber<nav_msgs::Odometry> imu_sub(n, "odom",
// 1);
// typedef
// message_filters::sync_policies::ApproximateTime<nav_msgs::OccupancyGrid,
// nav_msgs::Odometry> MySyncPolicy;
// message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10),
// imu_sub, map_sub);
// sync.registerCallback(boost::bind(&MapGenerator::mapCallback, this, _1,
// _2));
// ros::spin();
}
void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr &map,
const nav_msgs::Odometry::ConstPtr &odom) {
// ROS_INFO("Received a %d X %d map @ %.3f m/pix", map->info.width,
// map->info.height, map->info.resolution);
std::string mapdatafile = mapname_ + ".pgm";
// ROS_INFO("Writing map occupancy data to %s", mapdatafile.c_str());
FILE *out = fopen(mapdatafile.c_str(), "w");
if (!out) {
// ROS_ERROR("Couldn't save map file to %s", mapdatafile.c_str());
return;
}
fprintf(out, "P5\n# CREATOR: map_saver.cpp %.3f m/pix\n%d %d\n255\n",
map->info.resolution, map->info.width, map->info.height);
for (unsigned int y = 0; y < map->info.height; y++) {
for (unsigned int x = 0; x < map->info.width; x++) {
unsigned int i = x + (map->info.height - y - 1) * map->info.width;
if (map->data[i] >= 0 && map->data[i] <= threshold_free_) { // [0,free)
fputc(254, out);
} else if (map->data[i] >= threshold_occupied_) { // (occ,255]
fputc(000, out);
} else { // occ [0.25,0.65]
fputc(205, out);
}
}
}
fclose(out);
std::string mapmetadatafile = mapname_ + ".yaml";
// ROS_INFO("Writing map occupancy data to %s", mapmetadatafile.c_str());
FILE *yaml = fopen(mapmetadatafile.c_str(), "w");
/*
resolution: 0.100000
origin: [0.000000, 0.000000, 0.000000]
#
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
*/
// get origin pose
geometry_msgs::Quaternion orientation = map->info.origin.orientation;
tf2::Matrix3x3 mat(tf2::Quaternion(orientation.x, orientation.y,
orientation.z, orientation.w));
double yaw, pitch, roll;
mat.getEulerYPR(yaw, pitch, roll);
// get end pose
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
geometry_msgs::Quaternion rot;
ros::Time now = ros::Time::now();
try {
// tfBuffer.canTransform("/map", "/odom", now, ros::Duration(1.0));
rot = tfBuffer.lookupTransform("/map", "/odom", now).transform.rotation;
// ROS_INFO(rot);
} catch (tf2::TransformException &ex) {
// ROS_ERROR("lookupTransform Error: %s", ex.what());
}
// the incoming geometry_msgs::Quaternion is transformed to a tf2::Quaterion
tf2::Matrix3x3 mat_end(tf2::Quaternion(rot.x, rot.y, rot.z, rot.w));
// the tf::Quaternion has a method to acess roll pitch and yaw
double roll_end, pitch_end, yaw_end;
mat_end.getEulerYPR(yaw_end, pitch_end, roll_end);
fprintf(yaml,
"image: %s\nresolution: %f\norigin: [%f, %f, %f]\nend: [%f, %f, "
"%f]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n\n",
mapdatafile.c_str(), map->info.resolution,
map->info.origin.position.x, map->info.origin.position.y, yaw,
odom->pose.pose.position.x, odom->pose.pose.position.y, yaw_end);
// fprintf(yaml, "image: %s\nresolution: %f\norigin: [%f, %f, %f]\nnegate:
// 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n\n",
// mapdatafile.c_str(), map->info.resolution,
// map->info.origin.position.x, map->info.origin.position.y, yaw);
fclose(yaml);
ROS_INFO("Done\n");
saved_map_ = true;
}
};
#define USAGE \
"Usage: \n" \
" map_saver -h\n" \
" map_saver [--occ <threshold_occupied>] [--free <threshold_free>] [-f " \
"<mapname>] [ROS remapping args]"
int main(int argc, char **argv) {
ros::init(argc, argv, "map_saver");
std::string mapname = "map";
int threshold_occupied = 65;
int threshold_free = 25;
for (int i = 1; i < argc; i++) {
if (!strcmp(argv[i], "-h")) {
puts(USAGE);
return 0;
} else if (!strcmp(argv[i], "-f")) {
if (++i < argc)
mapname = argv[i];
else {
puts(USAGE);
return 1;
}
} else if (!strcmp(argv[i], "--occ")) {
if (++i < argc) {
threshold_occupied = std::atoi(argv[i]);
if (threshold_occupied < 1 || threshold_occupied > 100) {
ROS_ERROR("threshold_occupied must be between 1 and 100");
return 1;
}
} else {
puts(USAGE);
return 1;
}
} else if (!strcmp(argv[i], "--free")) {
if (++i < argc) {
threshold_free = std::atoi(argv[i]);
if (threshold_free < 0 || threshold_free > 100) {
ROS_ERROR("threshold_free must be between 0 and 100");
return 1;
}
} else {
puts(USAGE);
return 1;
}
} else {
puts(USAGE);
return 1;
}
}
if (threshold_occupied <= threshold_free) {
ROS_ERROR("threshold_free must be smaller than threshold_occupied");
return 1;
}
MapGenerator mg(mapname, threshold_occupied, threshold_free);
while (!mg.saved_map_ && ros::ok())
ros::spinOnce();
return 0;
}
Scanning dependencies of target map_server-map_saver
[ 50%] Building CXX object navigation/map_server/CMakeFiles/map_server-map_saver.dir/src/map_saver.cpp.o
In file included from /usr/include/boost/function/detail/maybe_include.hpp:22,
from /usr/include/boost/function/detail/function_iterate.hpp:14,
from /usr/include/boost/preprocessor/iteration/detail/iter/forward1.hpp:53,
from /usr/include/boost/function.hpp:71,
from /opt/ros/noetic/include/ros/forwards.h:40,
from /opt/ros/noetic/include/ros/common.h:37,
from /opt/ros/noetic/include/ros/ros.h:43,
from /home/user/catkin_ws/src/navigation/map_server/src/map_saver.cpp:36:
/usr/include/boost/function/function_template.hpp: In instantiation of 'static void boost::detail::function::void_function_invoker1<FunctionPtr, R, T0>::invoke(boost::detail::function::function_buffer&, T0) [with FunctionPtr = MapGenerator*; R = void; T0 = const boost::shared_ptr<const nav_msgs::Odometry_<std::allocator<void> > >&]':
/usr/include/boost/function/function_template.hpp:931:38: required from 'void boost::function1<R, T1>::assign_to(Functor) [with Functor = MapGenerator*; R = void; T0 = const boost::shared_ptr<const nav_msgs::Odometry_<std::allocator<void> > >&]'
/usr/include/boost/function/function_template.hpp:720:7: required from 'boost::function1<R, T1>::function1(Functor, typename boost::enable_if_<(! boost::is_integral<Functor>::value), int>::type) [with Functor = MapGenerator*; R = void; T0 = const boost::shared_ptr<const nav_msgs::Odometry_<std::allocator<void> > >&; typename boost::enable_if_<(! boost::is_integral<Functor>::value), int>::type = int]'
/usr/include/boost/function/function_template.hpp:1068:16: required from 'boost::function<R(T0)>::function(Functor, typename boost::enable_if_<(! boost::is_integral<Functor>::value), int>::type) [with Functor = MapGenerator*; R = void; T0 = const boost::shared_ptr<const nav_msgs::Odometry_<std::allocator<void> > >&; typename boost::enable_if_<(! boost::is_integral<Functor>::value), int>::type = int]'
/home/user/catkin_ws/src/navigation/map_server/src/map_saver.cpp:67:67: required from here
/usr/include/boost/function/function_template.hpp:117:11: error: 'f' cannot be used as a function
117 | BOOST_FUNCTION_RETURN(f(BOOST_FUNCTION_ARGS));
| ^~~~~~~~~~~~~~~~~~~~~
In file included from /usr/include/boost/bind.hpp:22,
from /opt/ros/noetic/include/ros/publisher.h:35,
from /opt/ros/noetic/include/ros/node_handle.h:32,
from /opt/ros/noetic/include/ros/ros.h:45,
from /home/user/catkin_ws/src/navigation/map_server/src/map_saver.cpp:36:
/usr/include/boost/bind/bind.hpp: In instantiation of 'void boost::_bi::list3<A1, A2, A3>::operator()(boost::_bi::type<void>, F&, A&, int) [with F = boost::_mfi::mf2<void, MapGenerator, const boost::shared_ptr<const nav_msgs::OccupancyGrid_<std::allocator<void> > >&, const boost::shared_ptr<const nav_msgs::Odometry_<std::allocator<void> > >&>; A = boost::_bi::rrlist1<const boost::shared_ptr<const nav_msgs::OccupancyGrid_<std::allocator<void> > >&>; A1 = boost::_bi::value<MapGenerator*>; A2 = boost::arg<1>; A3 = boost::arg<2>]':
/usr/include/boost/bind/bind.hpp:1306:50: required from 'boost::_bi::bind_t<R, F, L>::result_type boost::_bi::bind_t<R, F, L>::operator()(A1&&) [with A1 = const boost::shared_ptr<const nav_msgs::OccupancyGrid_<std::allocator<void> > >&; R = void; F = boost::_mfi::mf2<void, MapGenerator, const boost::shared_ptr<const nav_msgs::OccupancyGrid_<std::allocator<void> > >&, const boost::shared_ptr<const nav_msgs::Odometry_<std::allocator<void> > >&>; L = boost::_bi::list3<boost::_bi::value<MapGenerator*>, boost::arg<1>, boost::arg<2> >; boost::_bi::bind_t<R, F, L>::result_type = void]'
/usr/include/boost/function/function_template.hpp:158:11: required from 'static void boost::detail::function::void_function_obj_invoker1<FunctionObj, R, T0>::invoke(boost::detail::function::function_buffer&, T0) [with FunctionObj = boost::_bi::bind_t<void, boost::_mfi::mf2<void, MapGenerator, const boost::shared_ptr<const nav_msgs::OccupancyGrid_<std::allocator<void> > >&, const boost::shared_ptr<const nav_msgs::Odometry_<std::allocator<void> > >&>, boost::_bi::list3<boost::_bi::value<MapGenerator*>, boost::arg<1>, boost::arg<2> > >; R = void; T0= const boost::shared_ptr<const nav_msgs::OccupancyGrid_<std::allocator<void> > >&]'
/usr/include/boost/function/function_template.hpp:931:38: required from 'void boost::function1<R, T1>::assign_to(Functor) [with Functor = boost::_bi::bind_t<void, boost::_mfi::mf2<void, MapGenerator, const boost::shared_ptr<const nav_msgs::OccupancyGrid_<std::allocator<void> > >&, const boost::shared_ptr<const nav_msgs::Odometry_<std::allocator<void> > >&>, boost::_bi::list3<boost::_bi::value<MapGenerator*>, boost::arg<1>, boost::arg<2> > >; R = void; T0 = const boost::shared_ptr<const nav_msgs::OccupancyGrid_<std::allocator<void> > >&]'
/usr/include/boost/function/function_template.hpp:720:7: required from 'boost::function1<R, T1>::function1(Functor, typename boost::enable_if_<(! boost::is_integral<Functor>::value), int>::type) [with Functor = boost::_bi::bind_t<void, boost::_mfi::mf2<void, MapGenerator, const boost::shared_ptr<const nav_msgs::OccupancyGrid_<std::allocator<void> > >&, const boost::shared_ptr<const nav_msgs::Odometry_<std::allocator<void> > >&>, boost::_bi::list3<boost::_bi::value<MapGenerator*>, boost::arg<1>, boost::arg<2> > >; R = void; T0 = const boost::shared_ptr<const nav_msgs::OccupancyGrid_<std::allocator<void> > >&; typename boost::enable_if_<(! boost::is_integral<Functor>::value), int>::type = int]'
/usr/include/boost/function/function_template.hpp:1068:16: required from 'boost::function<R(T0)>::function(Functor, typename boost::enable_if_<(! boost::is_integral<Functor>::value), int>::type) [with Functor = boost::_bi::bind_t<void, boost::_mfi::mf2<void, MapGenerator, const boost::shared_ptr<const nav_msgs::OccupancyGrid_<std::allocator<void> > >&, const boost::shared_ptr<const nav_msgs::Odometry_<std::allocator<void> > >&>, boost::_bi::list3<boost::_bi::value<MapGenerator*>, boost::arg<1>, boost::arg<2> > >; R = void; T0 = const boost::shared_ptr<const nav_msgs::OccupancyGrid_<std::allocator<void> > >&; typename boost::enable_if_<(! boost::is_integral<Functor>::value), int>::type = int]'
/home/user/catkin_ws/src/navigation/map_server/src/map_saver.cpp:69:76: required from here
/usr/include/boost/bind/bind.hpp:398:75: error: no match for 'operator[]' (operand types are 'boost::_bi::rrlist1<const boost::shared_ptr<const nav_msgs::OccupancyGrid_<std::allocator<void> > >&>' and 'boost::arg<2>()')
398 | unwrapper<F>::unwrap(f, 0)(a[base_type::a1_], a[base_type::a2_], a[base_type::a3_]);
| ~^
/usr/include/boost/bind/bind.hpp:882:11: note: candidate: 'A1&& boost::_bi::rrlist1<A1>::operator[](boost::arg<1> (*)()) const [with A1 = const boost::shared_ptr<const nav_msgs::OccupancyGrid_<std::allocator<void> > >&]' <near match>
882 | A1 && operator[] (boost::arg<1> (*) ()) const { return std::forward<A1>( a1_ ); }
| ^~~~~~~~
/usr/include/boost/bind/bind.hpp:882:11: note: conversion of argument 1 would be ill-formed:
/usr/include/boost/bind/bind.hpp:398:75: error: invalid conversion from 'boost::arg<2> (*)()' to 'boost::arg<1> (*)()' [-fpermissive]
398 | unwrapper<F>::unwrap(f, 0)(a[base_type::a1_], a[base_type::a2_], a[base_type::a3_]);
| ~^
| |
| boost::arg<2> (*)()
/usr/include/boost/bind/bind.hpp:880:11: note: candidate: 'A1&& boost::_bi::rrlist1<A1>::operator[](boost::arg<1>) const [with A1 = const boost::shared_ptr<const nav_msgs::OccupancyGrid_<std::allocator<void> > >&]'
880 | A1 && operator[] (boost::arg<1>) const { return std::forward<A1>( a1_ ); } // not static_cast because of g++ 4.9
| ^~~~~~~~
/usr/include/boost/bind/bind.hpp:880:23: note: no known conversion for argument 1 from 'boost::arg<2>()' to 'boost::arg<1>'
880 | A1 && operator[] (boost::arg<1>) const { return std::forward<A1>( a1_ ); } // not static_cast because of g++ 4.9
| ^~~~~~~~~~~~~
/usr/include/boost/bind/bind.hpp:884:27: note: candidate: 'template<class T> T& boost::_bi::rrlist1<A1>::operator[](boost::_bi::value<T>&) const [with T = T; A1 = const boost::shared_ptr<const nav_msgs::OccupancyGrid_<std::allocator<void> > >&]'
884 | template<class T> T & operator[] ( _bi::value<T> & v ) const { return v.get(); }
| ^~~~~~~~
/usr/include/boost/bind/bind.hpp:884:27: note: template argument deduction/substitution failed:
/usr/include/boost/bind/bind.hpp:398:75: note: mismatched types 'boost::_bi::value<T>' and 'boost::arg<2>()'
398 | unwrapper<F>::unwrap(f, 0)(a[base_type::a1_], a[base_type::a2_], a[base_type::a3_]);
| ~^
/usr/include/boost/bind/bind.hpp:886:33: note: candidate: 'template<class T> const T& boost::_bi::rrlist1<A1>::operator[](const boost::_bi::value<T>&) const [with T = T; A1 = const boost::shared_ptr<const nav_msgs::OccupancyGrid_<std::allocator<void> > >&]'
886 | template<class T> T const & operator[] ( _bi::value<T> const & v ) const { return v.get(); }
| ^~~~~~~~
/usr/include/boost/bind/bind.hpp:886:33: note: template argument deduction/substitution failed:
/usr/include/boost/bind/bind.hpp:398:75: note: mismatched types 'const boost::_bi::value<T>' and 'boost::arg<2>()'
398 | unwrapper<F>::unwrap(f, 0)(a[base_type::a1_], a[base_type::a2_], a[base_type::a3_]);
| ~^
/usr/include/boost/bind/bind.hpp:888:27: note: candidate: 'template<class T> T& boost::_bi::rrlist1<A1>::operator[](const boost::reference_wrapper<FunctionObj>&) const [with T = T; A1 = const boost::shared_ptr<const nav_msgs::OccupancyGrid_<std::allocator<void> > >&]'
888 | template<class T> T & operator[] (reference_wrapper<T> const & v) const { return v.get(); }
| ^~~~~~~~
/usr/include/boost/bind/bind.hpp:888:27: note: template argument deduction/substitution failed:
/usr/include/boost/bind/bind.hpp:398:75: note: mismatched types 'const boost::reference_wrapper<T>' and 'boost::arg<2>()'
398 | unwrapper<F>::unwrap(f, 0)(a[base_type::a1_], a[base_type::a2_], a[base_type::a3_]);
| ~^
/usr/include/boost/bind/bind.hpp:890:76: note: candidate: 'template<class R, class F, class L> typename boost::_bi::result_traits<R, F>::type boost::_bi::rrlist1<A1>::operator[](boost::_bi::bind_t<R, F, L>&) const [with R= R; F = F; L = L; A1 = const boost::shared_ptr<const nav_msgs::OccupancyGrid_<std::allocator<void> > >&]'
890 | template<class R, class F, class L> typename result_traits<R, F>::type operator[] (bind_t<R, F, L>& b) const
| ^~~~~~~~
/usr/include/boost/bind/bind.hpp:890:76: note: template argument deduction/substitution failed:
/usr/include/boost/bind/bind.hpp:398:75: note: mismatched types 'boost::_bi::bind_t<R, F, L>' and 'boost::arg<2>()'
398 | unwrapper<F>::unwrap(f, 0)(a[base_type::a1_], a[base_type::a2_], a[base_type::a3_]);
| ~^
/usr/include/boost/bind/bind.hpp:896:76: note: candidate: 'template<class R, class F, class L> typename boost::_bi::result_traits<R, F>::type boost::_bi::rrlist1<A1>::operator[](const boost::_bi::bind_t<R, F, L>&) const [with R = R; F = F; L = L; A1 = const boost::shared_ptr<const nav_msgs::OccupancyGrid_<std::allocator<void> > >&]'
896 | template<class R, class F, class L> typename result_traits<R, F>::type operator[] (bind_t<R, F, L>const & b) const
| ^~~~~~~~
/usr/include/boost/bind/bind.hpp:896:76: note: template argument deduction/substitution failed:
/usr/include/boost/bind/bind.hpp:398:75: note: mismatched types 'const boost::_bi::bind_t<R, F, L>' and 'boost::arg<2>()'
398 | unwrapper<F>::unwrap(f, 0)(a[base_type::a1_], a[base_type::a2_], a[base_type::a3_]);
| ~^
make[2]: *** [navigation/map_server/CMakeFiles/map_server-map_saver.dir/build.make:63: navigation/map_server/CMakeFiles/map_server-map_saver.dir/src/map_saver.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:472: navigation/map_server/CMakeFiles/map_server-map_saver.dir/all] Error 2
make: *** [Makefile:141: all] Error 2
Invoking "make -j8 -l8" failed