//------------------------------------------------------------------------------
// atReleaseInit() -- Init weapon data at release
//------------------------------------------------------------------------------
void Missile::atReleaseInit()
{
// First the base class will setup the initial conditions
BaseClass::atReleaseInit();
if (getDynamicsModel() == 0) {
// set initial commands
cmdPitch = (LCreal) getPitch();
cmdHeading = (LCreal) getHeading();
cmdVelocity = vpMax;
if (getTargetTrack() != 0) {
// Set initial range and range dot
osg::Vec3 los = getTargetTrack()->getPosition();
trng = los.length();
trngT = trng;
}
else if (getTargetPlayer() != 0) {
// Set initial range and range dot
osg::Vec3 los = getTargetPosition();
trng = los.length();
trngT = trng;
}
else {
trng = 0;
}
// Range dot
trdot = 0;
trdotT = 0;
}
}
void MonsterFlock::end(void) {
FlockGroup::end();
mLast = mNow;
mNow.clear();
Vec2f o(
std::numeric_limits<float>::quiet_NaN(),
std::numeric_limits<float>::quiet_NaN()
);
for(FlockDict::iterator it = mLast.begin(); it != mLast.end(); ++it) {
FlockInfo &fi = it->second;
fi.target = getTargetPosition(fi.pos);
if(!o.isInvalid()) {
Vec2f d = fi.target - o;
float e = _NEAREST / (fabs(d.x) + fabs(d.y));
if(e > 1.0f) {
if(e > _PULL) e = _PULL;
fi.target = o + d * e;
clampInside(fi.target);
}
}
o = fi.target;
}
}
Position UnitClass::getPosition(int inFramesTime)
{
const Position ¤tPosition = getPosition();
const BWAPI::UnitType &type = getType();
if(type.topSpeed() != 0.0)
{
const Position &targetPosition = getTargetPosition();
int travelTime = int(currentPosition.getApproxDistance(targetPosition) / type.topSpeed());
if(inFramesTime > travelTime)
return targetPosition;
if(travelTime != 0)
{
float traveled = float(inFramesTime) / float(travelTime);
Vector direction = targetPosition - currentPosition;
direction *= traveled;
direction += Vector(mStoredPosition);
return direction;
}
}
return currentPosition;
}
void BountyMissionObjectiveImplementation::updateWaypoint() {
Locker locker(&syncMutex);
ManagedReference<MissionObject* > mission = this->mission.get();
WaypointObject* waypoint = mission->getWaypointToMission();
if (waypoint == NULL) {
Locker mlocker(mission);
waypoint = mission->createWaypoint();
}
Locker wplocker(waypoint);
waypoint->setPlanetCRC(getTargetZoneName().hashCode());
Vector3 position = getTargetPosition();
waypoint->setPosition(position.getX(), 0, position.getY());
waypoint->setActive(true);
mission->updateMissionLocation();
if (mission->getMissionLevel() == 1) {
getPlayerOwner().get()->sendSystemMessage("@mission/mission_bounty_informant:target_location_received"); // Target Waypoint Received.
}
}
bool isSpareBitOnDMIBus( ExtensibleChip * i_mcsChip, ExtensibleChip * i_mbChip )
{
bool bitOn = false;
do
{
// If any of these object is NULL, spare bit should not be on.
if ( ( NULL == i_mcsChip ) || ( NULL == i_mbChip ))
break;
// check spare deployed bit on Centaur side
SCAN_COMM_REGISTER_CLASS * dmiFir = i_mbChip->getRegister( "DMIFIR" );
int32_t rc = dmiFir->Read();
if ( SUCCESS != rc )
{
PRDF_ERR("isSpareBitOnDMIBus() : Failed to read DMIFIR."
"MEMBUF: 0x%08X", getHuid( i_mbChip->GetChipHandle()) );
break;
}
if ( dmiFir->IsBitSet( 9 ))
{
bitOn = true;
break;
}
// check spare deployed bit on Proc side
TargetHandle_t mcsTgt = i_mcsChip->GetChipHandle();
TargetHandle_t procTgt = getConnectedParent( mcsTgt, TYPE_PROC );
ExtensibleChip * procChip =
( ExtensibleChip * )systemPtr->GetChip( procTgt );
uint32_t mcsPos = getTargetPosition( mcsTgt );
const char * regStr = ( 4 > mcsPos) ? "IOMCFIR_0" : "IOMCFIR_1";
SCAN_COMM_REGISTER_CLASS * iomcFir = procChip->getRegister( regStr );
rc = iomcFir->Read();
if ( SUCCESS != rc )
{
PRDF_ERR("isSpareBitOnDMIBus() : Failed to read %s."
"MCS: 0x%08X", regStr, getHuid(mcsTgt) );
break;
}
// Bit 9, 17, 25 and 33 are for spare deployed.
// Check bit corrosponding to MCS position
uint8_t bitPos = 9 + ( mcsPos % 4 ) *8;
if ( iomcFir->IsBitSet(bitPos))
{
bitOn = true;
}
}while(0);
return bitOn;
}
geometry_msgs::Twist cob_cartesian_trajectories::getTwist(double dt, Frame F_current)
{
KDL::Frame F_target;
KDL::Frame F_diff;
geometry_msgs::Twist ControllTwist;
double start_roll, start_pitch, start_yaw = 0.0;
double current_roll, current_pitch, current_yaw = 0.0;
double target_roll, target_pitch, target_yaw = 0.0;
if(!bStarted)
{
F_start = F_current;
F_start.M.GetRPY(last_rpy_angles["target_roll"], last_rpy_angles["target_pitch"], last_rpy_angles["target_yaw"]);
F_start.M.GetRPY(last_rpy_angles["current_roll"], last_rpy_angles["current_pitch"], last_rpy_angles["current_yaw"]);
bStarted = true;
}
getTargetPosition(dt, F_target);
F_start.M.GetRPY(start_roll, start_pitch, start_yaw);
F_current.M.GetRPY(current_roll, current_pitch, current_yaw);
F_target.M.GetRPY(target_roll, target_pitch, target_yaw);
//std::cout << "AngleDiff: " << (current_yaw-start_yaw) << " vs " << (soll_angle) << " error: " << (soll_angle-current_yaw-start_yaw) << "\n";
std::cout << "Target (x,y): " << F_target.p.x() << ", " << F_target.p.y() << "\n";
std::cout << "Error (x,y): " << F_target.p.x()-F_current.p.x() << ", " << F_target.p.y()-F_current.p.y() << "\n";
std::cout << "Start (x,y): " << F_start.p.x() << ", " << F_start.p.y() << "\n";
std::cout << "Current (x,y): " << F_current.p.x() << ", " << F_current.p.y() << "\n";
// calling PIDController to calculate the actuating variable and get back twist
ControllTwist = PIDController(dt, F_target, F_current);
//DEBUG
F_diff.p.x(F_target.p.x()-F_current.p.x());
F_diff.p.y(F_target.p.y()-F_current.p.y());
F_diff.p.z(F_target.p.z()-F_current.p.z());
geometry_msgs::PoseArray poses;
poses.poses.resize(3);
tf::PoseKDLToMsg(F_current, poses.poses[0]);
tf::PoseKDLToMsg(F_target, poses.poses[1]);
tf::PoseKDLToMsg(F_diff, poses.poses[2]);
debug_cart_pub_.publish(poses);
//std::cout << "Twist x: " << 0.1 * F_diff.p.x() << " y: " << 0.1 * F_diff.p.y() << "\n";
//
return ControllTwist;
}
static Graph::Face *splitFace(Graph &graph, Graph::Face &face, FRandomStream &random) {
auto edgePair = getRandomOpposingEdges(face, random);
auto dir = getDirection(*edgePair.first);
// Assumes both edges are opposing faces on a rectangle.
auto otherDir = getDirection(*edgePair.second);
check((dir.x == -1 * otherDir.x) && (dir.y == -1 * otherDir.y));
// If the rectangle is not big enough do not split it.
if ((std::abs(dir.x) < 2*MARGIN+1) && (std::abs(dir.y) < 2*MARGIN+1))
return nullptr;
// Get a random point along the edges.
auto randX = (dir.x != 0 ? signOf(dir.x) * random.RandRange(MARGIN, std::abs(dir.x) - MARGIN) : 0);
auto randY = (dir.y != 0 ? signOf(dir.y) * random.RandRange(MARGIN, std::abs(dir.y) - MARGIN) : 0);
auto position0 = getSourcePosition(*edgePair.first) + Graph::Position{randX, randY};
auto position1 = getTargetPosition(*edgePair.second) + Graph::Position{randX, randY};
// Split the edges and connect.
Graph::Node &node0 = graph.SplitEdge(position0, *edgePair.first);
Graph::Node &node1 = graph.SplitEdge(position1, *edgePair.second);
return &graph.ConnectNodes(node0, node1);
}
void BountyMissionObjectiveImplementation::spawnTarget(const String& zoneName) {
Locker locker(&syncMutex);
ManagedReference<MissionObject* > mission = this->mission.get();
if ((npcTarget != NULL && npcTarget->isInQuadTree()) || isPlayerTarget()) {
return;
}
ZoneServer* zoneServer = getPlayerOwner().get()->getZoneServer();
Zone* zone = zoneServer->getZone(zoneName);
CreatureManager* cmng = zone->getCreatureManager();
if (npcTarget == NULL) {
Vector3 position = getTargetPosition();
try {
npcTarget = cast<AiAgent*>(zone->getCreatureManager()->spawnCreatureWithAi(mission->getTargetOptionalTemplate().hashCode(), position.getX(), zone->getHeight(position.getX(), position.getY()), position.getY(), 0));
} catch (Exception& e) {
fail();
ManagedReference<CreatureObject*> player = getPlayerOwner();
if (player != NULL) {
player->sendSystemMessage("ERROR: could not find template for target. Please report this on Mantis to help us track down the root cause.");
}
error("Template error: " + e.getMessage() + " Template = '" + mission->getTargetOptionalTemplate() +"'");
}
if (npcTarget != NULL) {
npcTarget->setCustomObjectName(mission->getTargetName(), true);
//TODO add observer to catch player kill and fail mission in that case.
addObserverToCreature(ObserverEventType::OBJECTDESTRUCTION, npcTarget);
addObserverToCreature(ObserverEventType::DAMAGERECEIVED, npcTarget);
} else {
fail();
ManagedReference<CreatureObject*> player = getPlayerOwner();
if (player != NULL) {
player->sendSystemMessage("ERROR: could not find template for target. Please report this on Mantis to help us track down the root cause.");
}
error("Could not spawn template: '" + mission->getTargetOptionalTemplate() + "'");
}
}
}
void UnitClass::drawUnitPosition()
{
const Position &pos = getPosition();
const BWAPI::UnitType &type = getType();
Player player = getPlayer();
const int barHeight = 4;
if((!isCompleted() || isMorphing()) && accessibility() != AccessType::Prediction)
{
double progress = getCompletedTime() - BWAPI::Broodwar->getFrameCount();
if(isMorphing())
progress /= getBuildType().buildTime();
else
progress /= type.buildTime();
progress = 1.0 - progress;
BWAPI::Color barColour = isMorphing() ? BWAPI::Colors::Red : BWAPI::Colors::Purple;
Position bottomLeft(pos.x() - type.dimensionLeft(), pos.y() + type.dimensionDown() + barHeight - 1);
DrawingHelper::Instance().drawProgressBar(bottomLeft, type.dimensionLeft()+type.dimensionRight(), barHeight, progress, barColour, player->getColor());
}
if(type.maxShields() > 0)
{
double progress = getShield();
progress /= type.maxShields();
Position bottomLeft(pos.x() - type.dimensionLeft(), pos.y() - type.dimensionUp() - barHeight + 2);
DrawingHelper::Instance().drawProgressBar(bottomLeft, type.dimensionLeft()+type.dimensionRight(), barHeight, progress, BWAPI::Colors::Blue, player->getColor());
}
if(type.maxHitPoints() > 0)
{
double progress = getHealth();
progress /= type.maxHitPoints();
Position bottomLeft(pos.x() - type.dimensionLeft(), pos.y() - type.dimensionUp() + 1);
DrawingHelper::Instance().drawProgressBar(bottomLeft, type.dimensionLeft()+type.dimensionRight(), barHeight, progress, BWAPI::Colors::Green, player->getColor());
}
BWAPI::Broodwar->drawBox(BWAPI::CoordinateType::Map, pos.x() - type.dimensionLeft(), pos.y() - type.dimensionUp(), pos.x() + type.dimensionRight(), pos.y() + type.dimensionDown(), player->getColor());
BWAPI::Broodwar->drawTextMap(pos.x() + type.dimensionRight(), pos.y(), "%s", player->getName().c_str());
AccessType access = accessibility();
BWAPI::Broodwar->drawTextMap(pos.x() + type.dimensionRight(), pos.y()+10, "%s", AccessType::getName(access.underlying()).c_str());
int existTime = getExistTime() - BWAPI::Broodwar->getFrameCount();
int completeTime = getCompletedTime() - BWAPI::Broodwar->getFrameCount() - existTime;
BWAPI::Broodwar->drawTextMap(pos.x() + type.dimensionRight(), pos.y()+20, "%d : %d", existTime, completeTime);
if(isMorphing())
BWAPI::Broodwar->drawTextMap(pos.x() + type.dimensionRight(), pos.y()+30, "Morphing");
else if(isCompleted())
BWAPI::Broodwar->drawTextMap(pos.x() + type.dimensionRight(), pos.y()+30, "Completed");
Position target = getTargetPosition();
BWAPI::Broodwar->drawLine(BWAPI::CoordinateType::Map, pos.x(), pos.y(), target.x(), target.y(), player->getColor());
}
void controller::legController(int leg_id, int phase) {
cout << endl << "Leg Controller = " << leg_id << endl;
if(swingStart[leg_id] == phase) {
cout << "Starting swing phase"<< endl;
computePlacementLocation(leg_id, lfHeight[0]);
setFootLocation(leg_id, phase);
}
else if(isInSwing(leg_id)) {
cout << "Swing phase"<< endl;
swingFlag[leg_id] = true;
//Get target position
setFootLocation(leg_id, phase);
vector<float> targetPosition = getTargetPosition(leg_id);
//Get link lengths
vector<float> lengths(2);
for(int i = 0; i < 2; i++) {
lengths[i] = body_bag->getFootLinkLength(leg_id, i);
}
//Set axis perpendicular to the kinematic chain plane
Eigen::MatrixXf axis = Eigen::MatrixXf::Random(4, 1);
axis(0, 0) = 0;
axis(1, 0) = 0;
if(leg_id % 2 == 0) {
axis(2, 0) = 1;
}
else {
axis(2, 0) = -1;
}
axis(3, 0) = 1;
//Get transformation matrices
vector<Eigen::MatrixXf> transformationMatrices(2);
Eigen::MatrixXf alignment = Eigen::MatrixXf::Identity(4, 4);
alignment(1, 1) = 0;
alignment(1, 2) = 1;
alignment(2, 1) = -1;
alignment(2, 2) = 0;
Eigen::MatrixXf mr = Eigen::MatrixXf::Identity(4, 4);
const dReal *rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 3));
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 4; j++) {
mr(i, j) = rotation_matrix_ode[i*4+j];
}
}
mr(3, 0) = 0;
mr(3, 1) = 0;
mr(3, 2) = 0;
mr(3, 3) = 1;
Eigen::MatrixXf mr2 = Eigen::MatrixXf::Identity(4, 4);
const dReal *rotation_matrix_ode2 = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 3));
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 4; j++) {
mr2(i, j) = rotation_matrix_ode2[i*4+j];
}
}
mr2(3, 0) = 0;
mr2(3, 1) = 0;
mr2(3, 2) = 0;
mr2(3, 3) = 1;
transformationMatrices[0] = mr*alignment.inverse();
transformationMatrices[1] = mr2*alignment.inverse();
//Get translation matrix
const dReal *center_location = dBodyGetPosition(body_bag->getFootLinkBody(leg_id,0)); //get location of the center of the link
Eigen::MatrixXf mt = Eigen::MatrixXf::Identity(4, 4); //translate to the start of the link
mt(1, 3) = -body_bag->getFootLinkLength(leg_id, 0)/2;
mr = Eigen::MatrixXf::Identity(4, 4); //get orientation
rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 0));
for(int k = 0; k < 3; k++) {
for(int j = 0; j < 4; j++) {
mr(k, j) = rotation_matrix_ode[k*4+j];
}
}
mr(3, 0) = 0;
mr(3, 1) = 0;
mr(3, 2) = 0;
mr(3, 3) = 1;
Eigen::MatrixXf origin = Eigen::MatrixXf::Random(4, 1);
origin(0, 0) = 0;
origin(1, 0) = 0;
origin(2, 0) = 0;
origin(3, 0) = 1;
Eigen::MatrixXf addition = alignment.inverse()*mr.inverse()*mt*origin; //part to add to the center location
Eigen::MatrixXf translationMatrix = Eigen::MatrixXf::Identity(4, 4);
translationMatrix(0, 3) = center_location[0] + addition(0, 0);
translationMatrix(1, 3) = center_location[1] + addition(1, 0);
translationMatrix(2, 3) = center_location[2] + addition(2, 0);
vector<float> angles = body_bag->getAngles(0);
Eigen::MatrixXf jointAngleChange = applyIK(lengths, transformationMatrices, angles, targetPosition, translationMatrix, axis);
//Use PD controllers to get torque
//link 1
float torque = foot_link_gain_kp[leg_id][0]*jointAngleChange(0,0) + foot_link_gain_kd[leg_id][0]*(jointAngleChange(0,0) - foot_link_pd_error[leg_id][0]);
dBodyAddTorque(body_bag->getFootLinkBody(leg_id,0), axis(0, 0)*torque, axis(1, 0)*torque, axis(2, 0)*torque);
for(int i = 0; i < 3; i++) {
swing_torque[leg_id][i] = axis(i, 0)*torque;
}
foot_link_pd_error[leg_id][0] = jointAngleChange(0,0);
//link 2
torque = foot_link_gain_kp[leg_id][1]*jointAngleChange(1,0) + foot_link_gain_kd[leg_id][1]*(jointAngleChange(1,0) - foot_link_pd_error[leg_id][1]);
dBodyAddTorque(body_bag->getFootLinkBody(leg_id,1), axis(0, 0)*torque, axis(1, 0)*torque, axis(2, 0)*torque);
foot_link_pd_error[leg_id][1] = jointAngleChange(1,0);
//link 3
rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 2));
for(int k = 0; k < 3; k++) {
for(int j = 0; j < 4; j++) {
mr(k, j) = rotation_matrix_ode[k*4+j];
}
}
mr(3, 0) = 0;
mr(3, 1) = 0;
mr(3, 2) = 0;
mr(3, 3) = 1;
float swing_phase = computeSwingPhase(leg_id, phase);
float error = ((30*M_PI)/180)*(1 - swing_phase) - ((60*M_PI)/180)*swing_phase - acos(mr(0, 0));
torque = foot_link_gain_kp[leg_id][2]*error + foot_link_gain_kd[leg_id][2]*(error - foot_link_pd_error[leg_id][2]);
dBodyAddTorque(body_bag->getFootLinkBody(leg_id,2), axis(0, 0)*torque, axis(1, 0)*torque, axis(2, 0)*torque);
foot_link_pd_error[leg_id][2] = error;
//link 4
rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 3));
for(int k = 0; k < 3; k++) {
for(int j = 0; j < 4; j++) {
mr(k, j) = rotation_matrix_ode[k*4+j];
}
}
mr(3, 0) = 0;
mr(3, 1) = 0;
mr(3, 2) = 0;
mr(3, 3) = 1;
swing_phase = computeSwingPhase(leg_id, phase);
error = ((90*M_PI)/180)*(1 - swing_phase) - ((30*M_PI)/180)*swing_phase - acos(mr(0, 0));
torque = foot_link_gain_kp[leg_id][3]*error + foot_link_gain_kd[leg_id][3]*(error - foot_link_pd_error[leg_id][3]);
dBodyAddTorque(body_bag->getFootLinkBody(leg_id,3), axis(0, 0)*torque, axis(1, 0)*torque, axis(2, 0)*torque);
foot_link_pd_error[leg_id][3] = error;
//Apply gravity compensation
float force = 9.810*body_bag->getDensity()*(body_bag->getFootLinkLength(leg_id, 0) + body_bag->getFootLinkLength(leg_id, 1) + body_bag->getFootLinkLength(leg_id, 2) + body_bag->getFootLinkLength(leg_id, 3));
dBodyAddForce(body_bag->getFootLinkBody(leg_id, 2), 0.0, 9.810*body_bag->getDensity()*body_bag->getFootLinkLength(leg_id, 2), 0.0);
if(leg_id < 2) {
dBodyAddForce(body_bag->getBackLink1Body(), 0.0, force, 0.0);
}
else {
dBodyAddForce(body_bag->getBackLink6Body(), 0.0, force, 0.0);
}
}
else {
cout << "Stance phase"<< endl;
swingFlag[leg_id] = false;
/*for(int i = 0; i < 3; i++){
swing_torque[leg_id][i] = 0;
}*/
stanceLegTreatment(leg_id);
}
}
Vec3f ConfuseSpell::getPosition() {
return getTargetPosition();
}
Vec3f ColdProtectionSpell::getPosition() {
return getTargetPosition();
}
Vec3f FireProtectionSpell::getPosition() {
return getTargetPosition();
}
Vec3f BlessSpell::getPosition() {
return getTargetPosition();
}
static Graph::Position getDirection(const Graph::HalfEdge &edge) {
return getTargetPosition(edge) - getSourcePosition(edge);
}
Vec3f ParalyseSpell::getPosition() {
return getTargetPosition();
}
Vec3f SlowDownSpell::getPosition() {
return getTargetPosition();
}
void HoleDetector::getImageCallback(const sensor_msgs::ImageConstPtr& original_image)
{
cv_bridge::CvImagePtr cv_ptr;
try {
cv_ptr = cv_bridge::toCvCopy(original_image, enc::BGR8);
}
catch (cv_bridge::Exception& e){
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::imshow("original", cv_ptr->image);
cv::Mat grayMat;
// Convert it to gray
cv::cvtColor( cv_ptr->image, grayMat, CV_BGR2GRAY );
cv::imshow("grayscale", grayMat);
// Convert to binary
cv::Mat binaryMat(grayMat.size(), grayMat.type());
cv::Mat binaryMatTest(grayMat.size(), grayMat.type());
//Apply thresholding
// cv::threshold(grayMat, binaryMat, 100, 255, CV_THRESH_BINARY | CV_THRESH_OTSU);
cv::threshold(grayMat, binaryMat, 100, 255, CV_THRESH_BINARY);
cv::threshold(grayMat, binaryMatTest, 200, 255, CV_THRESH_BINARY);
cv::imshow("binary", binaryMat);
// cvMoveWindow(WINDOW, 50, 50);
// Reduce the noise so we avoid false circle detection
int erosion_type = cv::MORPH_ELLIPSE;
int ksize1 =2;
int ksize2 = 4;
cv::Mat kernel1 = cv::getStructuringElement(erosion_type,cv::Size(ksize1,ksize1));
cv::Mat kernel2 = cv::getStructuringElement(erosion_type,cv::Size(ksize2,ksize2));
cv::dilate(binaryMat, binaryMat, kernel1);
cv::erode(binaryMat, binaryMat, kernel2);
// cv::Canny(src_gray, src_gray, 5, 70, 3);
// GaussianBlur(grayMat, grayMat, cv::Size(7, 7), 2, 2 );
// GaussianBlur(src_gray, src_gray, cv::Size(9, 9), 2, 2 );
cv::imshow("binary_filtered", binaryMat);
// Apply the Hough Transform to find the circles
// cv::HoughCircles(src_gray, mCircles, CV_HOUGH_GRADIENT, 2, 32.0, 50, 140, 0, 0 );
// Draw the circles detected
// for(size_t i = 0; i < mCircles.size(); i++ )
// {
// cv::Point center(cvRound(mCircles[i][0]), cvRound(mCircles[i][1]));
// int radius = cvRound(mCircles[i][2]);
// // circle center
// circle(cv_ptr->image, center, 3, cv::Scalar(0,255,0), -1, 8, 0 );
// circleare an external function as friend of a class, thus allowing this function to have
// circle(cv_ptr->image, center, radius, cv::Scalar(0,0,255), 3, 8, 0 );
// }
//Display the Data in the OpenCV HighGUI Window.
// cv::imshow(WINDOW, binaryMat);
cv::SimpleBlobDetector::Params params;
params.filterByColor = true;
params.blobColor = 0;
params.minDistBetweenBlobs = 5.0f;
params.filterByInertia = true;
params.minInertiaRatio = 0.2;
params.maxInertiaRatio = 1.0;
params.filterByConvexity = false;
params.minConvexity = 1.0;
params.maxConvexity = 10.0;
params.filterByCircularity = false;
params.filterByArea = true;
params.minArea = 3.0f;
params.maxArea = 400.0f;
cv::Ptr<cv::FeatureDetector> blob_detector = new cv::SimpleBlobDetector(params);
blob_detector->create("SimpleBlob");
std::vector<cv::KeyPoint> keypoints;
// blob_detector->detect(cv_ptr->image, keypoints);
blob_detector->detect(binaryMat, keypoints);
for (int i=0; i<keypoints.size(); i++){
float X=keypoints[i].pt.x;
float Y=keypoints[i].pt.y;
cv::circle(cv_ptr->image,cv::Point(X,Y),5,cv::Scalar(0,255,0),-1);
}
cv::imshow("simple_blob", cv_ptr->image);
cv::waitKey(3);
/* Set tolerance as fraction of smallest image dimension*/
double coeff = 0.5;
int width = cv_ptr->image.cols;
int height = cv_ptr->image.rows;
const double tol = coeff * std::min(width,height);
float center[] = { width*0.5, height*0.5};
float toolXY[2];
float targetXY[] = {0.0, 0.0};
/* Set tool tip to center for now */
toolXY[0] = center[0];
toolXY[1] = center[1];
if( getTargetPosition (keypoints, toolXY, targetXY, tol) ) {
hole_detection::PositionXY target_msg;
target_msg.X = targetXY[0] - toolXY[0];
target_msg.Y = targetXY[1] - toolXY[1];
target_msg.header.stamp = ros::Time::now();
TargetPosition.publish(target_msg);
cv::circle(cv_ptr->image,cv::Point(toolXY[0],toolXY[1]),5,cv::Scalar(255,0,0),-1);
cv::circle(cv_ptr->image,cv::Point(targetXY[0],targetXY[1]),5,cv::Scalar(0,0,255),-1);
}
cv::imshow("targeted", cv_ptr->image);
mImagePub.publish(cv_ptr->toImageMsg());
}
Vec3f IncinerateSpell::getPosition() {
return getTargetPosition();
}