void loadGame(void)
{
//On charge les données pour la map
initMaps();
}
MrfData_LCD::MrfData_LCD( const u_int32_t& blocklength, const u_int32_t& defaultindex, const u_int32_t& defaultstreamoffset, const u_int32_t& defaultvalueoffset, const bool& defaultreverse, const bool& defaultstreamreverse )
: TMrfDataAdvBase (blocklength, defaultindex, defaultstreamoffset, defaultvalueoffset, defaultreverse, defaultstreamreverse),
TMrfDataAdv1D (blocklength, defaultindex, defaultstreamoffset, defaultvalueoffset, defaultreverse, defaultstreamreverse)
{
//_upper_lower = 'u';
initMaps();
}
int main(){
#ifdef LOCAL_TEST
std::ifstream in("input.txt",std::ifstream::in);
#else
std::istream& in=std::cin;
#endif
std::tr1::unordered_map<char,std::string> charToAscii(1024);
std::tr1::unordered_map<std::string,char> asciiToChar(1024);
initMaps(charToAscii,asciiToChar);
std::string line;
for(;;){
std::getline(in,line);
if(!in) break;
if(line.empty()) {
std::cout<<"\n";
continue;
}
const bool isEncode=!isdigit(line[0]);
std::cout<<(isEncode?encode(line,charToAscii):decode(line,asciiToChar))<<"\n";
}
#ifdef LOCAL_TEST
in.close();
#endif
return 0;
}
glmFamily::glmFamily(List ll) throw (std::runtime_error)
: lst(ll),
// d_family(as<std::string>(wrap(ll["family"]))),
// d_link(as<std::string>(wrap(ll["link"]))),
// I haven't been able to work out an expression to initialize the
// Functions from list components. This is a placeholder until I can
// do so.
d_devRes("c"), d_linkfun("c"), d_linkinv("c"),
d_muEta("c"), d_variance("c") {
// d_devRes(wrap(ll["dev.resids"])),
// d_linkfun(wrap(ll["linkfun"])),
// d_linkinv(wrap(ll["linkinv"])),
// d_muEta(wrap(ll["mu.eta"])),
// d_variance(wrap(ll["variance"])) {
if (as<string>(lst.attr("class")) != "family")
throw std::runtime_error("glmFamily requires a list of (S3) class \"family\"");
CharacterVector ff = lst["family"], lnk = lst["link"];
d_family = as<std::string>(ff);
d_link = as<std::string>(lnk);
d_linkinv = ll["linkinv"];
d_linkfun = ll["linkfun"];
d_muEta = ll["mu.eta"];
d_variance = ll["variance"];
d_devRes = ll["dev.resids"];
if (!lnks.count("identity")) initMaps();
}
void MapManager::init() {
initTilesets();
initMaps();
Map::scrollX = 0;
Map::scrollY = 0;
}
Map::~Map()
{
x = 16;
y = 16;
choice = 1;
fileName = "";
path = "";
initMaps();
}
void Costmap2D::copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y, double win_size_x, double win_size_y){
//check for self windowing
if(this == &map){
ROS_ERROR("Cannot convert this costmap into a window of itself");
return;
}
//clean up old data
deleteMaps();
deleteKernels();
//compute the bounds of our new map
unsigned int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
if(!map.worldToMap(win_origin_x, win_origin_y, lower_left_x, lower_left_y)
|| ! map.worldToMap(win_origin_x + win_size_x, win_origin_y + win_size_y, upper_right_x, upper_right_y)){
ROS_ERROR("Cannot window a map that the window bounds don't fit inside of");
return;
}
size_x_ = upper_right_x - lower_left_x;
size_y_ = upper_right_y - lower_left_y;
resolution_ = map.resolution_;
origin_x_ = win_origin_x;
origin_y_ = win_origin_y;
ROS_DEBUG("ll(%d, %d), ur(%d, %d), size(%d, %d), origin(%.2f, %.2f)",
lower_left_x, lower_left_y, upper_right_x, upper_right_y, size_x_, size_y_, origin_x_, origin_y_);
//initialize our various maps and reset markers for inflation
initMaps(size_x_, size_y_);
//copy the window of the static map and the costmap that we're taking
copyMapRegion(map.costmap_, lower_left_x, lower_left_y, map.size_x_, costmap_, 0, 0, size_x_, size_x_, size_y_);
copyMapRegion(map.static_map_, lower_left_x, lower_left_y, map.size_x_, static_map_, 0, 0, size_x_, size_x_, size_y_);
max_obstacle_range_ = map.max_obstacle_range_;
max_obstacle_height_ = map.max_obstacle_height_;
max_raytrace_range_ = map.max_raytrace_range_;
inscribed_radius_ = map.inscribed_radius_;
circumscribed_radius_ = map.circumscribed_radius_;
inflation_radius_ = map.inflation_radius_;
cell_inscribed_radius_ = map.cell_inscribed_radius_;
cell_circumscribed_radius_ = map.cell_circumscribed_radius_;
cell_inflation_radius_ = map.cell_inflation_radius_;
//set the cost for the circumscribed radius of the robot
circumscribed_cost_lb_ = map.circumscribed_cost_lb_;
weight_ = map.weight_;
//copy the cost and distance kernels
copyKernels(map, cell_inflation_radius_);
}
Map::Map()
{
x = 16;
y = 16;
choice = 1;
fileName = "";
path = "";
setWallThickness(1, 1);
initMaps();
}
void Costmap2D::replaceFullMap(double win_origin_x, double win_origin_y,
unsigned int data_size_x, unsigned int data_size_y,
const std::vector<unsigned char>& static_data){
//delete our old maps
deleteMaps();
//update the origin and size of the new map
size_x_ = data_size_x;
size_y_ = data_size_y;
origin_x_ = win_origin_x;
origin_y_ = win_origin_y;
//initialize our various maps
initMaps(size_x_, size_y_);
//make sure the inflation queue is empty at the beginning of the cycle (should always be true)
ROS_ASSERT_MSG(inflation_queue_.empty(), "The inflation queue must be empty at the beginning of inflation");
unsigned int index = 0;
unsigned char* costmap_index = costmap_;
std::vector<unsigned char>::const_iterator static_data_index = static_data.begin();
//copy static data into the costmap
for(unsigned int i = 0; i < size_y_; ++i){
for(unsigned int j = 0; j < size_x_; ++j){
//check if the static value is above the unknown or lethal thresholds
if(track_unknown_space_ && unknown_cost_value_ > 0 && *static_data_index == unknown_cost_value_)
*costmap_index = NO_INFORMATION;
else if(*static_data_index >= lethal_threshold_)
*costmap_index = LETHAL_OBSTACLE;
else
*costmap_index = FREE_SPACE;
if(*costmap_index == LETHAL_OBSTACLE){
unsigned int mx, my;
indexToCells(index, mx, my);
enqueue(index, mx, my, mx, my, inflation_queue_);
}
++costmap_index;
++static_data_index;
++index;
}
}
//now... let's inflate the obstacles
inflateObstacles(inflation_queue_);
//we also want to keep a copy of the current costmap as the static map
memcpy(static_map_, costmap_, size_x_ * size_y_ * sizeof(unsigned char));
}
Costmap2D& Costmap2D::operator=(const Costmap2D& map) {
//check for self assignement
if(this == &map)
return *this;
//clean up old data
deleteMaps();
deleteKernels();
size_x_ = map.size_x_;
size_y_ = map.size_y_;
resolution_ = map.resolution_;
origin_x_ = map.origin_x_;
origin_y_ = map.origin_y_;
//initialize our various maps
initMaps(size_x_, size_y_);
//copy the static map
memcpy(static_map_, map.static_map_, size_x_ * size_y_ * sizeof(unsigned char));
//copy the cost map
memcpy(costmap_, map.costmap_, size_x_ * size_y_ * sizeof(unsigned char));
max_obstacle_range_ = map.max_obstacle_range_;
max_obstacle_height_ = map.max_obstacle_height_;
max_raytrace_range_ = map.max_raytrace_range_;
inscribed_radius_ = map.inscribed_radius_;
circumscribed_radius_ = map.circumscribed_radius_;
inflation_radius_ = map.inflation_radius_;
cell_inscribed_radius_ = map.cell_inscribed_radius_;
cell_circumscribed_radius_ = map.cell_circumscribed_radius_;
cell_inflation_radius_ = map.cell_inflation_radius_;
//set the cost for the circumscribed radius of the robot
circumscribed_cost_lb_ = map.circumscribed_cost_lb_;
weight_ = map.weight_;
//copy the cost and distance kernels
copyKernels(map, cell_inflation_radius_);
return *this;
}
void Costmap2D::reshapeStaticMap(double win_origin_x, double win_origin_y,
unsigned int data_size_x, unsigned int data_size_y, const std::vector<unsigned char>& static_data){
int m_ox, m_oy;
worldToMapNoBounds(win_origin_x, win_origin_y, m_ox, m_oy);
//compute the bounds for the size of our new map
int bl_x = std::min(m_ox, 0);
int bl_y = std::min(m_oy, 0);
int ur_x = std::max(m_ox + data_size_x, size_x_);
int ur_y = std::max(m_oy + data_size_y, size_y_);
//create a temporary map to hold our static data and copy the old static map into it
unsigned char* static_map_copy = new unsigned char[size_x_ * size_y_];
memcpy(static_map_copy, static_map_, size_x_ * size_y_ * sizeof(unsigned char));
//delete our old maps... the user will lose any
//cost information not stored in the static map when reshaping a map
deleteMaps();
//update the origin and sizes, and cache data we'll need
double old_origin_x = origin_x_;
double old_origin_y = origin_y_;
unsigned int old_size_x = size_x_;
unsigned int old_size_y = size_y_;
size_x_ = ur_x - bl_x;
size_y_ = ur_y - bl_y;
origin_x_ = std::min(origin_x_, win_origin_x);
origin_y_ = std::min(origin_y_, win_origin_y);
//initialize our various maps
initMaps(size_x_, size_y_);
//reset our maps to be full of unknown space if appropriate
resetMaps();
//now, copy the old static map into the new costmap
unsigned int start_x, start_y;
worldToMap(old_origin_x, old_origin_y, start_x, start_y);
copyMapRegion(static_map_copy, 0, 0, old_size_x, costmap_, start_x, start_y, size_x_, old_size_x, old_size_y);
delete[] static_map_copy;
//now we want to update the map with the new static map data
replaceStaticMapWindow(win_origin_x, win_origin_y, data_size_x, data_size_y, static_data);
}
LevelReader::LevelReader()
{
initMaps();
}
GameStateController::GameStateController()
{
initMaps();
}
void RGBDImageProc::RGBDCallback(
const ImageMsg::ConstPtr& rgb_msg,
const ImageMsg::ConstPtr& depth_msg,
const CameraInfoMsg::ConstPtr& rgb_info_msg,
const CameraInfoMsg::ConstPtr& depth_info_msg)
{
boost::mutex::scoped_lock(mutex_);
// for profiling
double dur_unwarp, dur_rectify, dur_reproject, dur_cloud, dur_allocate;
// **** initialize if needed
if (!initialized_)
{
initMaps(rgb_info_msg, depth_info_msg);
initialized_ = true;
}
// **** convert ros images to opencv Mat
cv_bridge::CvImageConstPtr rgb_ptr = cv_bridge::toCvShare(rgb_msg);
cv_bridge::CvImageConstPtr depth_ptr = cv_bridge::toCvShare(depth_msg);
const cv::Mat& rgb_img = rgb_ptr->image;
const cv::Mat& depth_img = depth_ptr->image;
//cv::imshow("RGB", rgb_img);
//cv::imshow("Depth", depth_img);
//cv::waitKey(1);
// **** rectify
ros::WallTime start_rectify = ros::WallTime::now();
cv::Mat rgb_img_rect, depth_img_rect;
cv::remap(rgb_img, rgb_img_rect, map_rgb_1_, map_rgb_2_, cv::INTER_LINEAR);
cv::remap(depth_img, depth_img_rect, map_depth_1_, map_depth_2_, cv::INTER_NEAREST);
dur_rectify = getMsDuration(start_rectify);
//cv::imshow("RGB Rect", rgb_img_rect);
//cv::imshow("Depth Rect", depth_img_rect);
//cv::waitKey(1);
// **** unwarp
if (unwarp_)
{
ros::WallTime start_unwarp = ros::WallTime::now();
unwarpDepthImage(depth_img_rect, coeff_0_rect_, coeff_1_rect_, coeff_2_rect_, fit_mode_);
dur_unwarp = getMsDuration(start_unwarp);
}
else dur_unwarp = 0.0;
// **** reproject
ros::WallTime start_reproject = ros::WallTime::now();
cv::Mat depth_img_rect_reg;
buildRegisteredDepthImage(intr_rect_depth_, intr_rect_rgb_, ir2rgb_,
depth_img_rect, depth_img_rect_reg);
dur_reproject = getMsDuration(start_reproject);
// **** point cloud
if (publish_cloud_)
{
ros::WallTime start_cloud = ros::WallTime::now();
PointCloudT::Ptr cloud_ptr;
cloud_ptr.reset(new PointCloudT());
buildPointCloud(depth_img_rect_reg, rgb_img_rect, intr_rect_rgb_, *cloud_ptr);
cloud_ptr->header = rgb_info_msg->header;
cloud_publisher_.publish(cloud_ptr);
dur_cloud = getMsDuration(start_cloud);
}
else dur_cloud = 0.0;
// **** allocate registered rgb image
ros::WallTime start_allocate = ros::WallTime::now();
cv_bridge::CvImage cv_img_rgb(rgb_msg->header, rgb_msg->encoding, rgb_img_rect);
ImageMsg::Ptr rgb_out_msg = cv_img_rgb.toImageMsg();
// **** allocate registered depth image
cv_bridge::CvImage cv_img_depth(depth_msg->header, depth_msg->encoding, depth_img_rect_reg);
ImageMsg::Ptr depth_out_msg = cv_img_depth.toImageMsg();
// **** update camera info (single, since both images are in rgb frame)
rgb_rect_info_msg_.header = rgb_info_msg->header;
dur_allocate = getMsDuration(start_allocate);
ROS_INFO("Rect: %.1f Reproj: %.1f Unwarp: %.1f Cloud %.1f Alloc: %.1f ms",
dur_rectify, dur_reproject, dur_unwarp, dur_cloud, dur_allocate);
// **** publish
rgb_publisher_.publish(rgb_out_msg);
depth_publisher_.publish(depth_out_msg);
info_publisher_.publish(rgb_rect_info_msg_);
}
Comm::Comm(const Comm &comm)
: line_(comm.line_)
{
initMaps();
}
Comm::Comm()
{
initMaps();
}
void RGBDImageProc::RGBDCallback(
const ImageMsg::ConstPtr& rgb_msg,
const ImageMsg::ConstPtr& depth_msg,
const CameraInfoMsg::ConstPtr& rgb_info_msg,
const CameraInfoMsg::ConstPtr& depth_info_msg)
{
boost::mutex::scoped_lock(mutex_);
// for profiling
double dur_unwarp, dur_rectify, dur_reproject, dur_cloud, dur_allocate;
// **** images need to be the same size
if (rgb_msg->height != depth_msg->height ||
rgb_msg->width != depth_msg->width)
{
ROS_WARN("RGB and depth images have different sizes, skipping");
return;
}
// **** initialize if needed
if (size_in_.height != (int)rgb_msg->height ||
size_in_.width != (int)rgb_msg->width)
{
ROS_INFO("Initializing");
size_in_.height = (int)rgb_msg->height;
size_in_.width = (int)rgb_msg->width;
initMaps(rgb_info_msg, depth_info_msg);
}
// **** convert ros images to opencv Mat
cv_bridge::CvImageConstPtr rgb_ptr = cv_bridge::toCvShare(rgb_msg);
cv_bridge::CvImageConstPtr depth_ptr = cv_bridge::toCvShare(depth_msg);
const cv::Mat& rgb_img = rgb_ptr->image;
const cv::Mat& depth_img = depth_ptr->image;
//cv::imshow("RGB", rgb_img);
//cv::imshow("Depth", depth_img);
//cv::waitKey(1);
// **** rectify
ros::WallTime start_rectify = ros::WallTime::now();
cv::Mat rgb_img_rect, depth_img_rect;
cv::remap(rgb_img, rgb_img_rect, map_rgb_1_, map_rgb_2_, cv::INTER_LINEAR);
cv::remap(depth_img, depth_img_rect, map_depth_1_, map_depth_2_, cv::INTER_NEAREST);
dur_rectify = getMsDuration(start_rectify);
//cv::imshow("RGB Rect", rgb_img_rect);
//cv::imshow("Depth Rect", depth_img_rect);
//cv::waitKey(1);
// **** unwarp
if (unwarp_)
{
ros::WallTime start_unwarp = ros::WallTime::now();
rgbdtools::unwarpDepthImage(
depth_img_rect, coeff_0_rect_, coeff_1_rect_, coeff_2_rect_, fit_mode_);
dur_unwarp = getMsDuration(start_unwarp);
}
else dur_unwarp = 0.0;
// **** reproject
ros::WallTime start_reproject = ros::WallTime::now();
cv::Mat depth_img_rect_reg;
rgbdtools::buildRegisteredDepthImage(
intr_rect_depth_, intr_rect_rgb_, ir2rgb_, depth_img_rect, depth_img_rect_reg);
dur_reproject = getMsDuration(start_reproject);
// **** point cloud
if (publish_cloud_)
{
ros::WallTime start_cloud = ros::WallTime::now();
PointCloudT::Ptr cloud_ptr;
cloud_ptr.reset(new PointCloudT());
rgbdtools::buildPointCloud(
depth_img_rect_reg, rgb_img_rect, intr_rect_rgb_, *cloud_ptr);
cloud_ptr->header = rgb_info_msg->header;
cloud_publisher_.publish(cloud_ptr);
dur_cloud = getMsDuration(start_cloud);
}
else dur_cloud = 0.0;
// **** allocate registered rgb image
ros::WallTime start_allocate = ros::WallTime::now();
cv_bridge::CvImage cv_img_rgb(rgb_msg->header, rgb_msg->encoding, rgb_img_rect);
ImageMsg::Ptr rgb_out_msg = cv_img_rgb.toImageMsg();
// **** allocate registered depth image
cv_bridge::CvImage cv_img_depth(depth_msg->header, depth_msg->encoding, depth_img_rect_reg);
ImageMsg::Ptr depth_out_msg = cv_img_depth.toImageMsg();
// **** update camera info (single, since both images are in rgb frame)
rgb_rect_info_msg_.header = rgb_info_msg->header;
dur_allocate = getMsDuration(start_allocate);
// **** print diagnostics
double dur_total = dur_rectify + dur_reproject + dur_unwarp + dur_cloud + dur_allocate;
if(verbose_)
{
ROS_INFO("Rect %.1f Reproj %.1f Unwarp %.1f Cloud %.1f Alloc %.1f Total %.1f ms",
dur_rectify, dur_reproject, dur_unwarp, dur_cloud, dur_allocate,
dur_total);
}
// **** publish
rgb_publisher_.publish(rgb_out_msg);
depth_publisher_.publish(depth_out_msg);
info_publisher_.publish(rgb_rect_info_msg_);
}
