void PotentialField::target_waypoint_callback(
visualization_msgs::Marker::ConstPtr target_point_msgs) {
static TargetWaypointFieldParamater param;
double ver_x_p(param.ver_x_p);
double ver_y_p(param.ver_y_p);
ros::Time time = ros::Time::now();
geometry_msgs::PointStamped in, out;
in.header = target_point_msgs->header;
in.point = target_point_msgs->pose.position;
tf::TransformListener tflistener;
try {
ros::Time now = ros::Time(0);
tflistener.waitForTransform("/map", "/potential_field_link", now,
ros::Duration(10.0));
tflistener.transformPoint("/potential_field_link", in.header.stamp, in,
"/map", out);
} catch (tf::TransformException &ex) {
ROS_ERROR("%s", ex.what());
}
for (GridMapIterator it(map_); !it.isPastEnd(); ++it) {
Position position;
map_.getPosition(*it, position);
map_.at("target_waypoint_field", *it) = 0.0;
map_.at("target_waypoint_field", *it) -=
0.5 * std::exp((-1.0 * (std::pow((position.y() - (out.point.y)), 2.0) /
std::pow(2.0 * ver_y_p, 2.0))) +
(-1.0 * (std::pow((position.x() - (out.point.x)), 2.0) /
std::pow(2.0 * ver_x_p, 2.0))));
}
map_.setTimestamp(time.toNSec());
}
void populateMap(GridMap &map, string layer,string file_path, float scale,float res)
{
cv_bridge::CvImage cv_image;
Mat img = imread(file_path,CV_LOAD_IMAGE_GRAYSCALE);
cv::Size img_size= img.size();
cv::resize(img,img,cv::Size(),scale,scale, CV_INTER_CUBIC);
map.setGeometry(Length(img.rows*res,img.cols*res),res);
for (GridMapIterator it(map); !it.isPastEnd(); ++it)
{
Position position;
map.getPosition(*it, position);
int x = (img.rows/2) + position.x()/res+res/2.0;
int y = (img.cols/2) + position.y()/res+res/2.0;
map.at(layer, *it) = img.at<uchar>(x,y)<200?1:0;
}
map.setTimestamp(ros::Time::now().toNSec());
ROS_INFO("Created map with size %f x %f m (%i x %i cells).",map.getLength().x(), map.getLength().y(),map.getSize()(0), map.getSize()(1));
}
void PotentialField::vscan_points_callback(
sensor_msgs::PointCloud2::ConstPtr vscan_msg) {
static VscanPointsFieldParamater param;
double around_x(param.around_x);
double around_y(param.around_y);
ros::Time time = ros::Time::now();
pcl::PointCloud<pcl::PointXYZ> pcl_vscan;
pcl::fromROSMsg(*vscan_msg, pcl_vscan);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_vscan_ptr(
new pcl::PointCloud<pcl::PointXYZ>(pcl_vscan));
double length_x = map_.getLength().x() / 2.0;
double length_y = map_.getLength().y() / 2.0;
for (GridMapIterator it(map_); !it.isPastEnd(); ++it) {
Position position;
map_.getPosition(*it, position);
map_.at("vscan_points_field", *it) = 0.0;
for (int i(0); i < (int)pcl_vscan.size(); ++i) {
double point_x = pcl_vscan.at(i).x - map_x_offset_;
if (3.0 < pcl_vscan.at(i).z + tf_z_ || pcl_vscan.at(i).z + tf_z_ < 0.3)
continue;
if (length_x < point_x && point_x < -1.0 * length_x)
continue;
if (length_y < pcl_vscan.at(i).y && pcl_vscan.at(i).y < -1.0 * length_y)
continue;
if ((point_x + tf_x_) - around_x < position.x() &&
position.x() < point_x + tf_x_ + around_x) {
if (pcl_vscan.at(i).y - around_y < position.y() &&
position.y() < pcl_vscan.at(i).y + around_y) {
map_.at("vscan_points_field", *it) = 1.0; // std::exp(0.0) ;
}
}
}
}
map_.setTimestamp(time.toNSec());
publish_potential_field();
}
void PotentialField::obj_callback(
autoware_msgs::DetectedObjectArray::ConstPtr obj_msg) { // Create grid map.
static ObstacleFieldParameter param;
double ver_x_p(param.ver_x_p);
double ver_y_p(param.ver_y_p);
// Add data to grid map.
ros::Time time = ros::Time::now();
for (GridMapIterator it(map_); !it.isPastEnd(); ++it) {
Position position;
map_.getPosition(*it, position);
map_.at("obstacle_field", *it) = 0.0;
for (int i(0); i < (int)obj_msg->objects.size(); ++i) {
double pos_x =
obj_msg->objects.at(i).pose.position.x + tf_x_ - map_x_offset_;
double pos_y = obj_msg->objects.at(i).pose.position.y;
double len_x = obj_msg->objects.at(i).dimensions.x / 2.0;
double len_y = obj_msg->objects.at(i).dimensions.y / 2.0;
double r, p, y;
tf::Quaternion quat(obj_msg->objects.at(i).pose.orientation.x,
obj_msg->objects.at(i).pose.orientation.y,
obj_msg->objects.at(i).pose.orientation.z,
obj_msg->objects.at(i).pose.orientation.w);
tf::Matrix3x3(quat).getRPY(r, p, y);
double rotated_pos_x = std::cos(-1.0 * y) * (position.x() - pos_x) -
std::sin(-1.0 * y) * (position.y() - pos_y) +
pos_x;
double rotated_pos_y = std::sin(-1.0 * y) * (position.x() - pos_x) +
std::cos(-1.0 * y) * (position.y() - pos_y) +
pos_y;
if (-0.5 < pos_x && pos_x < 4.0) {
if (-1.0 < pos_y && pos_y < 1.0)
continue;
}
if (pos_x - len_x < rotated_pos_x && rotated_pos_x < pos_x + len_x) {
if (pos_y - len_y < rotated_pos_y && rotated_pos_y < pos_y + len_y) {
map_.at("obstacle_field", *it) =
std::max(std::exp(0.0),
static_cast<double>(map_.at("obstacle_field", *it)));
} else if (rotated_pos_y < pos_y - len_y) {
map_.at("obstacle_field", *it) = std::max(
std::exp(
(-1.0 * (std::pow((rotated_pos_y - (pos_y - len_y)), 2.0) /
std::pow(2.0 * ver_y_p, 2.0)))),
static_cast<double>(map_.at("obstacle_field", *it)));
} else if (pos_y + len_y < rotated_pos_y) {
map_.at("obstacle_field", *it) = std::max(
std::exp(
(-1.0 * (std::pow((rotated_pos_y - (pos_y + len_y)), 2.0) /
std::pow(2.0 * ver_y_p, 2.0)))),
static_cast<double>(map_.at("obstacle_field", *it)));
}
} else if (rotated_pos_x < pos_x - len_x) {
if (rotated_pos_y < pos_y - len_y) {
map_.at("obstacle_field", *it) = std::max(
std::exp(
(-1.0 * (std::pow((rotated_pos_y - (pos_y - len_y)), 2.0) /
std::pow(2.0 * ver_y_p, 2.0))) +
(-1.0 * (std::pow((rotated_pos_x - (pos_x - len_x)), 2.0) /
std::pow(2.0 * ver_x_p, 2.0)))),
static_cast<double>(map_.at("obstacle_field", *it)));
} else if (pos_y + len_y < rotated_pos_y) {
map_.at("obstacle_field", *it) = std::max(
std::exp(
(-1.0 * (std::pow((rotated_pos_y - (pos_y + len_y)), 2.0) /
std::pow(2.0 * ver_y_p, 2.0))) +
(-1.0 * (std::pow((rotated_pos_x - (pos_x - len_x)), 2.0) /
std::pow(2.0 * ver_x_p, 2.0)))),
static_cast<double>(map_.at("obstacle_field", *it)));
} else if (pos_y - len_y < rotated_pos_y &&
rotated_pos_y < pos_y + len_y) {
map_.at("obstacle_field", *it) = std::max(
std::exp(
(-1.0 * (std::pow((rotated_pos_x - (pos_x - len_x)), 2.0) /
std::pow(2.0 * ver_x_p, 2.0)))),
static_cast<double>(map_.at("obstacle_field", *it)));
}
} else if (pos_x + len_x < rotated_pos_x) {
if (rotated_pos_y < pos_y - len_y) {
map_.at("obstacle_field", *it) = std::max(
std::exp(
(-1.0 * (std::pow((rotated_pos_y - (pos_y - len_y)), 2.0) /
std::pow(2.0 * ver_y_p, 2.0))) +
(-1.0 * (std::pow((rotated_pos_x - (pos_x + len_x)), 2.0) /
std::pow(2.0 * ver_x_p, 2.0)))),
static_cast<double>(map_.at("obstacle_field", *it)));
} else if (pos_y + len_y / 2.0 < rotated_pos_y) {
map_.at("obstacle_field", *it) = std::max(
std::exp(
(-1.0 * (std::pow((rotated_pos_y - (pos_y + len_y)), 2.0) /
std::pow(2.0 * ver_y_p, 2.0))) +
(-1.0 * (std::pow((rotated_pos_x - (pos_x + len_x)), 2.0) /
std::pow(2.0 * ver_x_p, 2.0)))),
static_cast<double>(map_.at("obstacle_field", *it)));
} else if (pos_y - len_y < rotated_pos_y &&
rotated_pos_y < pos_y + len_y) {
map_.at("obstacle_field", *it) = std::max(
std::exp(
(-1.0 * (std::pow((rotated_pos_x - (pos_x + len_x)), 2.0) /
std::pow(2.0 * ver_x_p, 2.0)))),
static_cast<double>(map_.at("obstacle_field", *it)));
}
}
}
}
// Publish grid map.
map_.setTimestamp(time.toNSec());
publish_potential_field();
}
