void Life::draw(const Camera &camera)
{
if(anim != NULL)
anim->draw(Object::getPosition() - point_to_vector(camera.getPosition()), etc::ALIGN_BY_CENTER, IntRect(0, 0, -1, -1));
}
bool PixelMap::isCollision(const RectMap &rectmap, const IntPoint &pos1, const IntPoint &pos2, const IntRect &rect1, const IntRect &rect2, const Alignment &alignment1, const Alignment &alignment2) const
{
IntPoint corner1(pos1), corner2(pos2);
IntRect framerect1, framerect2;
IntRect mrect1, mrect2;
IntRect *cutrect;
/* Break */
if(map == NULL)
return false;
/* Translate negative axises */
mrect1 = rect_translate_negative_axises(rect1, size);
mrect2 = rect_translate_negative_axises(rect2, rectmap.getSize());
/* Align */
if(alignment1.getX() != 0) corner1.decX((int)(mrect1.getWidth() * alignment1.getX()));
if(alignment1.getY() != 0) corner1.decY((int)(mrect1.getHeight() * alignment1.getY()));
if(alignment2.getX() != 0) corner2.decX((int)(mrect2.getWidth() * alignment2.getX()));
if(alignment2.getY() != 0) corner2.decY((int)(mrect2.getHeight() * alignment2.getY()));
/* Framerects */
framerect1.load(corner1.getX(), corner1.getY(), mrect1.getWidth(), mrect1.getHeight());
framerect2.load(corner2.getX(), corner2.getY(), mrect2.getWidth(), mrect2.getHeight());
/* Break */
if(!size_to_rect(size).isCovering(mrect1))
throw Exception() << "pixelmap1 doesn't fully cover rect";
/* Cutrect */
if((cutrect = framerect1.getCutrect(framerect2)) == NULL)
return false;
/* Iterate rects, check pixels */
for(int r = 0; r < rectmap.getRectCount(); r++)
{
int fpx, fpy; /* First pixel to check */
int lpx, lpy; /* Last pixel to check */
IntRect srect = rectmap.getRect(r) + point_to_vector(corner2) - IntVector(mrect2.getX(), mrect2.getY());
IntRect *scutrect;
/* Clip with cutrect*/
if((scutrect = srect.getCutrect(*cutrect)) == NULL)
continue;
/* Pixels to check */
fpx = scutrect->getX() - (corner1.getX() - mrect1.getX());
fpy = scutrect->getY() - (corner1.getY() - mrect1.getY());
lpx = fpx + scutrect->getWidth() - 1;
lpy = fpy + scutrect->getHeight() - 1;
/* Check pixels */
for(int py = fpy; py <= lpy; py++)
{
for(int px = fpx; px <= lpx; px++)
{
if(is_pixel(px, py))
{
/* Free mem */
delete cutrect;
delete scutrect;
/* Collision detected */
return true;
}
}
}
delete scutrect;
}
delete cutrect;
return false;
}
void costmap_cb(const nav_msgs::GridCells::ConstPtr& msg) {
ODOM_FRAME = msg->header.frame_id;
geometry_msgs::PoseArray ff_msg;
ff_msg.header.stamp = msg->header.stamp;
ff_msg.header.frame_id = ODOM_FRAME;
last_fv = msg->header.stamp;
force_vector[0] = force_vector[1] = 0;
std::vector<geometry_msgs::Point> points = msg->cells;
std::vector<geometry_msgs::Pose> poses;
int count = 0;
float total_weight = 0.0;
for (std::vector<std::string>::size_type i = 0; i < points.size(); i++) {
geometry_msgs::PointStamped point, point_trans;
point.header.stamp = msg->header.stamp;
point.header.frame_id = ODOM_FRAME;
point.point.x = points[i].x;
point.point.y = points[i].y;
tf_listener->waitForTransform(BASE_FRAME, ODOM_FRAME, point.header.stamp, ros::Duration(0.1));
try {
tf_listener->transformPoint(BASE_FRAME, point, point_trans);
}
catch (tf::ExtrapolationException e) {
ROS_ERROR("Unable to get tf transform: %s", e.what());
return;
}
float x = point_trans.point.x;
float y = point_trans.point.y;
float yaw = atan(y / x);
if (x > 0) yaw = modulus(yaw + PI, 2*PI);
std::vector<float> point_vector = point_to_vector(x, y);
if (point_vector.size() > 0) {
count++;
//float weight = 0.75*(MAX_RANGE - BASE_RADIUS - CLEARING_DIST)/(sqrt(pow(x, 2) + pow(y, 2)) - BASE_RADIUS - CLEARING_DIST);
//if ( weight < 0 )
// std::cout << "evil" << std::endl;
//length += weight;
//float weight = (PI - abs(yaw)) / PI;
force_vector[0] += point_vector[0];
force_vector[1] += point_vector[1];
geometry_msgs::PoseStamped pose, pose_trans;
pose.header.stamp = msg->header.stamp;
pose.header.frame_id = BASE_FRAME;
pose.pose.position.x = x;
pose.pose.position.y = y;
pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
tf_listener->waitForTransform(ODOM_FRAME, BASE_FRAME, pose.header.stamp, ros::Duration(0.1));
try {
tf_listener->transformPose(ODOM_FRAME, pose, pose_trans);
}
catch (tf::ExtrapolationException e) {
ROS_ERROR("Unable to get tf transform: %s", e.what());
return;
}
poses.push_back(pose_trans.pose);
}
}
//force_vector[0] /= length + 1;
//force_vector[1] /= length + 1;
std::cout << force_vector[0] << "," << force_vector[1] << std::endl;
// publish resulting force vector as Pose
geometry_msgs::PoseStamped force, force_trans;
force.header.stamp = msg->header.stamp;
force.header.frame_id = BASE_FRAME;
float force_yaw = atan(force_vector[1] / force_vector[0]);
if (force_vector[0] > 0) force_yaw = modulus(force_yaw + PI, 2*PI);
force.pose.orientation = tf::createQuaternionMsgFromYaw(force_yaw);
tf_listener->waitForTransform(BASE_FRAME, ODOM_FRAME, msg->header.stamp, ros::Duration(0.1));
try {
tf_listener->transformPose(ODOM_FRAME, force, force_trans);
}
catch (tf::ExtrapolationException e) {
ROS_ERROR("Unable to get tf transform: %s", e.what());
return;
}
force_obst_pub.publish(force_trans);
// publish force field as PoseArray
ff_msg.poses = poses;
force_field_pub.publish(ff_msg);
}
