void generateHeadReference() {
if (goal_type_ == NONE) {
return;
}
if (goal_type_ == LOOKAT) {
targetToPanTilt(goal_target_, goal_pan_, goal_tilt_);
publishMarker(goal_target_);
}
if (fabs(goal_pan_ - current_pan_) < goal_error_tolerance_pan_ && fabs(goal_tilt_ - current_tilt_) < goal_error_tolerance_tilt_) {
// pan and tilt are within error bound
if (as_->isActive() && !keep_tracking_) {
amigo_head_ref::HeadRefResult result;
ROS_DEBUG("Target reached, ready for next head target!");
result.result = "Done";
as_->setSucceeded(result);
goal_type_ = NONE;
}
return;
}
// populate msg
sensor_msgs::JointState head_ref;
head_ref.name.push_back("neck_pan_joint");
head_ref.name.push_back("neck_tilt_joint");
head_ref.position.push_back(goal_pan_);
head_ref.position.push_back(goal_tilt_);
head_ref.velocity.push_back(goal_pan_vel_);
head_ref.velocity.push_back(goal_tilt_vel_);
//publish angles over ROS
head_pub_.publish(head_ref);
}
void
ArucoMapping::publishTfs(bool world_option)
{
for(int i = 0; i < marker_counter_; i++)
{
// Actual Marker
std::stringstream marker_tf_id;
marker_tf_id << "marker_" << i;
// Older marker - or World
std::stringstream marker_tf_id_old;
if(i == 0)
marker_tf_id_old << "world";
else
marker_tf_id_old << "marker_" << markers_[i].previous_marker_id;
broadcaster_.sendTransform(tf::StampedTransform(markers_[i].tf_to_previous,ros::Time::now(),marker_tf_id_old.str(),marker_tf_id.str()));
// Position of camera to its marker
std::stringstream camera_tf_id;
camera_tf_id << "camera_" << i;
broadcaster_.sendTransform(tf::StampedTransform(markers_[i].current_camera_tf,ros::Time::now(),marker_tf_id.str(),camera_tf_id.str()));
if(world_option == true)
{
// Global position of marker TF
std::stringstream marker_globe;
marker_globe << "marker_globe_" << i;
broadcaster_.sendTransform(tf::StampedTransform(markers_[i].tf_to_world,ros::Time::now(),"world",marker_globe.str()));
}
// Cubes for RVIZ - markers
publishMarker(markers_[i].geometry_msg_to_previous,markers_[i].marker_id,i);
}
// Global Position of object
if(world_option == true)
broadcaster_.sendTransform(tf::StampedTransform(world_position_transform_,ros::Time::now(),"world","camera_position"));
}
/*!
* @brief Callback for point clouds.
* Callback for point clouds. Uses PCL to find the centroid
* of the points in a box in the center of the point cloud.
* Publishes cmd_vel messages with the goal from the cloud.
* @param cloud The point cloud message.
*/
void cloudcb(const PointCloud::ConstPtr& cloud)
{
//X,Y,Z of the centroid
float x = 0.0;
float y = 0.0;
float z = 1e6;
//Number of points observed
unsigned int n = 0;
bool direction = 0 ;
//Iterate through all the points in the region and find the average of the position
BOOST_FOREACH (const pcl::PointXYZ& pt, cloud->points)
{
//First, ensure that the point's position is valid. This must be done in a seperate
//if because we do not want to perform comparison on a nan value.
if (!std::isnan(x) && !std::isnan(y) && !std::isnan(z))
{
//Test to ensure the point is within the aceptable box.
if (-pt.y > min_y_ && -pt.y < max_y_ && pt.x < max_x_ && pt.x > min_x_ && pt.z < max_z_)
{
//Add the point to the totals
x += pt.x;
y += pt.y;
z = std::min(z, pt.z);
n++;
}
}
}
//If there are points, find the centroid and calculate the command goal.
//If there are no points, simply publish a stop goal.
if (n>4000)
{
x /= n;
y /= n;
/*
if(z > max_z_){
ROS_DEBUG("No valid points detected, stopping the robot");
ROS_INFO("Too far");
if (enabled_)
{ //modified
geometry_msgs::TwistPtr cmd(new geometry_msgs::Twist());
cmd->linear.x = 0.2 ;
//cmd->angular.z = -x * x_scale_;
cmdpub_.publish(cmd);
//cmdpub_.publish(geometry_msgs::TwistPtr(new geometry_msgs::Twist()));
}
//return;
}
ROS_INFO("Centroid at %f %f %f with %d points", x, y, z, n);
publishMarker(x, y, z);
if(z < max_z_)*/
//{
//if(z != goal_z_)
//{
//ROS_INFO("************");
double rand_angular;
if (z-goal_z_ > 0){
if (enabled_){
ROS_INFO("near goal %f %f %f with %d points", x, y, z, n);
geometry_msgs::TwistPtr cmd(new geometry_msgs::Twist());
cmd->linear.x = 0.2 ;
if(x > 0.2){
direction = 1 ;
srand((unsigned)time(NULL)) ;
rand_angular = rand()%7 ;
rand_angular = rand_angular/7.0 ;
ROS_INFO(" x > 0.2 , rand_angular %f ", rand_angular);
if(rand_angular > 0.7 && rand_angular <= 1.0 ){
cmd->angular.z = 0.4 ;
ROS_INFO("rand_angular = 0.7~1.0, cmd->angular.z = %f ", cmd->angular.z );
}else if(rand_angular > 0.4 && rand_angular <= 0.7){
cmd->angular.z = rand_angular ;
ROS_INFO("rand_angular = 0.4~0.7, cmd->angular.z = %f ", cmd->angular.z );
}else{
cmd->angular.z = 0.3 ;
ROS_INFO("rand_angular = 0.0~0.4, cmd->angular.z = %f ", cmd->angular.z );
}
//(z - goal_z_) * z_scale_ * 0.5 ;
}else if(x <- 0.2){
direction = 0 ;
srand((unsigned)time(NULL)) ;
rand_angular = rand()%7 ;
rand_angular = double(rand_angular)/7.0 - 1.0 ;
ROS_INFO(" x < -0.2 , rand_angular %f ", rand_angular);
if(rand_angular >= -1.0 && rand_angular < -0.7 ){
cmd->angular.z = -0.36 ;
ROS_INFO("rand_angular = -1.0~-0.7, cmd->angular.z = %f ", cmd->angular.z );
}else if(rand_angular >= -0.7 && rand_angular < -0.5){
cmd->angular.z = rand_angular ;
ROS_INFO("rand_angular = -0.7~-0.4, cmd->angular.z = %f ", cmd->angular.z );
}else {
cmd->angular.z = - 0.2 ;
ROS_INFO("rand_angular = -0.4~-0.0, cmd->angular.z = %f ", cmd->angular.z );
}
}else{
if(direction){
cmd->angular.z = 0.3 ;
}else{
cmd->angular.z = -0.3 ;
}
}
//cmd->angular.z = -x * x_scale_ ;
cmdpub_.publish(cmd) ;
}
/*}else{
if(enabled_)
{
ROS_INFO("&&&&&&&&&&&&&&&&&&&&&&&");
geometry_msgs::TwistPtr cmd(new geometry_msgs::Twist());
cmd->linear.x = 0.3 ;
cmd->angular.z = 0.5 ;
cmdpub_.publish(cmd) ;
}
}*/
}else{
ROS_INFO("goal is bingo %f %f %f with %d points", x, y, z, n);
geometry_msgs::TwistPtr cmd(new geometry_msgs::Twist());
cmd->linear.x = 0 ;
int num = 0;
double ang = 0.0 ;
if(x > 0.2){
srand((unsigned)time(NULL));
num = (rand() % MAX);
ang = double(num) / MAX ;
ROS_INFO(" x > 0.2 , ang %f ", ang);
direction = 1 ;
if(ang > 0.7 && ang <= 1 ){
cmd->angular.z = 0.4 ;
ROS_INFO("ang=0.7~1.0, ancmd->angular.z = %f ", cmd->angular.z );
}else if(ang > 0.4 && ang <= 0.7){
cmd->angular.z = ang ;
ROS_INFO("ang=0.4~0.7, cmd->angular.z = %f ", cmd->angular.z );
}else{
cmd->angular.z = 0.3 ;
ROS_INFO("ang=0.0~0.4,cmd->angular.z = %f ", cmd->angular.z );
}
//(z - goal_z_) * z_scale_ * 0.5 ;
}else if(x < -0.2){
srand((unsigned)time(NULL));
num = (rand() % MAX);
ang = double(num) / MAX ;
ang = ang - 1.0 ;
direction = 0 ;
ROS_INFO(" x < -0.2 , ang %f ", ang);
if(ang >= -1.0 && ang < -0.7 ){
cmd->angular.z = -0.2 ;
ROS_INFO("ang=-1.0~-0.7, ancmd->angular.z = %f ", cmd->angular.z );
}else if(ang >= -0.7 && ang < -0.4){
cmd->angular.z = ang ;
ROS_INFO("ang=-0.7~-0.4, ancmd->angular.z = %f ", cmd->angular.z );
}else {
cmd->angular.z = - 0.2 ;
ROS_INFO("ang=-0.4~-0.0, ancmd->angular.z = %f ", cmd->angular.z );
}
}else{
if(direction){
cmd->angular.z = 0.3 ;
}else{
cmd->angular.z = -0.3 ;
}
}
cmdpub_.publish(cmd) ;
}
//cmd->angular.z = -x * x_scale_ ;
/*
if(fabs(x) > 0.2){
ROS_INFO("goal is bingo %f %f %f with %d points,x > 0.01", x, y, z, n);
cmd->angular.z = 3*x;// * x_scale_;
}else{
srand((unsigned)time(NULL));
int num = (rand() % MAX);
double ang = double(num) / MAX - 0.5 ;
ROS_INFO("random %f %f %f with %d points,x < 0.01,angular=%f", x, y, z, n,ang);
if(ang >= 0.2 && ang <= 0.5 ){
cmd->angular.z = ang ;
}else if(ang >= 0 && ang < 0.2){
cmd->angular.z = 0.2 ;
}else if(ang >= -0.2 && ang < 0){
cmd->angular.z = - 0.2 ;
}else{
cmd->angular.z = ang ;
}
}*/
//cmdpub_.publish(cmd) ;
//}
//}
}else{
ROS_DEBUG("No points detected, stopping the robot");
publishMarker(x, y, z);
if (enabled_)
{ //modified
geometry_msgs::TwistPtr cmd(new geometry_msgs::Twist());
ROS_INFO("There are no points!");
cmd->linear.x = 0.2 ;
//cmd->angular.z = 0.6 ;
//cmd->angular.z = -x * x_scale_;
cmdpub_.publish(cmd);
//cmdpub_.publish(geometry_msgs::TwistPtr(new geometry_msgs::Twist()));
}
}
publishBbox();
}
void goalCB() {
ROS_DEBUG("goalCB");
const amigo_head_ref::HeadRefGoalConstPtr &goal = as_->acceptNewGoal();
if (goal->goal_type == amigo_head_ref::HeadRefGoal::LOOKAT) {
tf::pointStampedMsgToTF(goal->target_point, goal_target_);
goal_target_.stamp_ = ros::Time();
targetToPanTilt(goal_target_, goal_pan_, goal_tilt_);
goal_type_ = LOOKAT;
keep_tracking_ = goal->keep_tracking;
publishMarker(goal_target_);
} else if (goal->goal_type == amigo_head_ref::HeadRefGoal::PAN_TILT) {
goal_pan_ = goal->pan;
goal_tilt_ = goal->tilt;
goal_type_ = PAN_TILT;
keep_tracking_ = false;
} else {
goal_type_ = NONE;
}
goal_pan_vel_ = goal->pan_vel;
goal_tilt_vel_ = goal->tilt_vel;
// set default min/max
double min_pan = min_pan_;
double max_pan = max_pan_;
double min_tilt = min_tilt_;
double max_tilt = max_tilt_;
// if set in action goal, override min/max
if (goal->min_pan != goal->max_pan) {
min_pan = goal->min_pan;
max_pan = goal->max_pan;
}
if (goal->min_tilt != goal->max_tilt) {
min_tilt = goal->min_tilt;
max_tilt = goal->max_tilt;
}
if (keep_tracking_) {
// constraint pan
if (goal_pan_ < min_pan) {
goal_pan_ = min_pan;
} else if (goal_pan_ > max_pan) {
goal_pan_ = max_pan;
}
// contraint tilt
if (goal_tilt_ < min_tilt) {
goal_tilt_ = min_tilt;
} else if (goal_tilt_ > max_tilt) {
goal_tilt_ = max_tilt;
}
} else {
if ((goal_pan_ < min_pan || goal_pan_ > max_pan
|| goal_tilt_ < min_tilt || goal_tilt_ > max_tilt)) {
// pan / tilt out of bounds
amigo_head_ref::HeadRefResult result;
result.result = "Pan / tilt out of bounds";
as_->setAborted(result);
goal_type_ = NONE;
}
}
}
void Collision_Handler::checkForPreviousCollision(int yValue, int zValue, moveit_msgs::CollisionObject& co)
{
// check for previous collisions and calculate diff
double yDiff = 0;
double zDiff = 0;
if(yValue != 0)
{
for(int i = 0; i < maxAttempts_; i++)
{
if(collisionValues_[i] == 0)
{
break;
}
else if (collisionValues_[i] == 1
|| collisionValues_[i] == 2
|| collisionValues_[i] == 3)
{
yDiff = 0.05 * (1.0/(i+1));
}
else if (collisionValues_[i] == 4
|| collisionValues_[i] == 8
|| collisionValues_[i] == 12)
{
yDiff = -0.05 * (1.0/(i+1));
}
ROS_WARN_STREAM("yDiff: " << yDiff << endl);
}
}
if(zValue != 0)
{
for(int i = 0; i < maxAttempts_; i++)
{
if(collisionValues_[i] == 0)
{
break;
}
else if (collisionValues_[i] == 1
|| collisionValues_[i] == 4
|| collisionValues_[i] == 5)
{
zDiff = 0.05 * (1.0/(i+1));
}
else if (collisionValues_[i] == 2
|| collisionValues_[i] == 8
|| collisionValues_[i] == 10)
{
zDiff = -0.05 * (1.0/(i+1));
}
}
}
// create new poseStamped with data from CollisionObject
geometry_msgs::PoseStamped pose;
pose.header.frame_id = co.id;
pose.header.stamp = co.header.stamp;
// check if position is in primitive_pose or mesh_pose
if(co.primitive_poses.size() == 1)
{
pose.pose.orientation.w = 1;
// ROS_WARN("Going to publish the marker BEFORE transformation, press any key and enter to continue");
// std::string input;
// std::cin >> input;
publishMarker(pose);
try{
if(rightArm_)
{
listener_->waitForTransform("/r_gripper_r_finger_tip_link", pose.header.frame_id, ros::Time(0), ros::Duration(3));
listener_->transformPose("/r_gripper_r_finger_tip_link", ros::Time(0), pose, co.header.frame_id, pose);
}
else
{
listener_->waitForTransform("/l_gripper_r_finger_tip_link", pose.header.frame_id, ros::Time(0), ros::Duration(3));
listener_->transformPose("/l_gripper_r_finger_tip_link", ros::Time(0), pose, co.header.frame_id, pose);
}
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
// set new frame in header
co.header.frame_id = pose.header.frame_id;
co.primitive_poses[0] = pose.pose;
// apply corrections
co.primitive_poses[0].position.y += yDiff;
co.primitive_poses[0].position.z += zDiff;
co.header.stamp = ros::Time(0);
// pose for marker
pose.pose.position.y += yDiff;
pose.pose.position.z += zDiff;
// ROS_WARN("Going to publish the marker AFTER transformation, press any key and enter to continue");
// std::cin >> input;
// publishMarker(pose);
}
// position in mesh_pose
else
{
pose.pose.orientation.w = 1;
pose.header.frame_id = co.id;
pose.header.stamp = co.header.stamp;
// ROS_WARN("Going to publish the marker BEFORE transformation, press any key and enter to continue");
// std::string input;
// std::cin >> input;
publishMarker(pose);
try{
if(rightArm_)
{
listener_->waitForTransform("/r_gripper_r_finger_tip_link", pose.header.frame_id, ros::Time(0), ros::Duration(3));
listener_->transformPose("/r_gripper_r_finger_tip_link", ros::Time(0), pose, co.header.frame_id, pose);
}
else
{
listener_->waitForTransform("/l_gripper_r_finger_tip_link", pose.header.frame_id, ros::Time(0), ros::Duration(3));
listener_->transformPose("/l_gripper_r_finger_tip_link", ros::Time(0), pose, co.header.frame_id, pose);
}
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
// set new frame in header
co.header.frame_id = pose.header.frame_id;
co.mesh_poses[0] = pose.pose;
// apply corrections
co.mesh_poses[0].position.y += yDiff;
co.mesh_poses[0].position.z += zDiff;
co.header.stamp = ros::Time(0);
// pose for marker
pose.pose.position.y += yDiff;
pose.pose.position.z += zDiff;
// ROS_WARN("Going to publish the marker AFTER transformation, press any key and enter to continue");
// std::cin >> input;
// publishMarker(pose);
}
// publish moved collisionObject
pi_->addObject(co);
}
void ARSinglePublisher::getTransformationCallback(const sensor_msgs::ImageConstPtr& image_msg)
{
// ROS_INFO("======================================================");
// ROS_INFO("Callback...");
ARMarkerInfo *marker_info;
int marker_num;
int i, k;
// Get the image from ROSTOPIC
// NOTE: the data_ptr format is BGR because the ARToolKit library was
// build with V4L, data_ptr format change according to the
// ARToolKit configure option (see config.h).
try
{
capture_ = bridge_.imgMsgToCv(image_msg, "bgr8");
}
catch (sensor_msgs::CvBridgeException & e)
{
ROS_ERROR("Could not convert from >%s< to 'bgr8'.", image_msg->encoding.c_str ());
return;
}
// const int WIDTH = 640;
// const int HEIGHT = 480;
// // declare a destination IplImage object with correct size, depth and channels
// IplImage *destination = cvCreateImage(cvSize(WIDTH, HEIGHT), capture_->depth, capture_->nChannels);
// // use cvResize to resize source to a destination image
// cvResize(capture_, destination);
// // save image with a name supplied with a second argument
// std::string filename = "/tmp/" + marker_frame_ + ".jpg";
// cvSaveImage(filename.c_str(), destination);
// ROS_INFO("BEFORE: Depth = >%i<.", capture_->depth);
// ROS_INFO("BEFORE: nChannels = >%i<.", capture_->nChannels);
// ROS_INFO("BEFORE: Width = >%i<.", capture_->width);
// ROS_INFO("BEFORE: WidthStep = >%i<.", capture_->widthStep);
// ROS_INFO("BEFORE: Height = >%i<.", capture_->height);
// ROS_INFO("BEFORE: ImageSize = >%i<.", capture_->imageSize);
// ROS_INFO("BEFORE: nSize = >%i<.", capture_->nSize);
// ROS_INFO("BEFORE: dataOrder = >%i<.", capture_->dataOrder);
// ROS_INFO("BEFORE: origin = >%i<.", capture_->origin);
// capture_ = destination;
// // memcpy(capture_->imageData, destination->imageData, destination->imageSize);
// ROS_INFO("AFTER: Depth = >%i<.", capture_->depth);
// ROS_INFO("AFTER: nChannels = >%i<.", capture_->nChannels);
// ROS_INFO("AFTER: Width = >%i<.", capture_->width);
// ROS_INFO("AFTER: WidthStep = >%i<.", capture_->widthStep);
// ROS_INFO("AFTER: Height = >%i<.", capture_->height);
// ROS_INFO("AFTER: ImageSize = >%i<.", capture_->imageSize);
// ROS_INFO("AFTER: nSize = >%i<.", capture_->nSize);
// ROS_INFO("AFTER: dataOrder = >%i<.", capture_->dataOrder);
// ROS_INFO("AFTER: origin = >%i<.", capture_->origin);
// cvConvertImage(capture_, capture_, CV_CVTIMG_FLIP); //flip image
ARUint8 *data_ptr = (ARUint8 *)capture_->imageData;
// detect the markers in the video frame
if (arDetectMarker(data_ptr, threshold_, &marker_info, &marker_num) < 0)
{
ROS_FATAL ("arDetectMarker failed");
ROS_BREAK (); // FIXME: I don't think this should be fatal... -Bill
}
// check for known patterns
k = -1;
for (i = 0; i < marker_num; i++)
{
if (marker_info[i].id == patt_id_)
{
ROS_DEBUG("Found pattern: %d ", patt_id_);
// make sure you have the best pattern (highest confidence factor)
if (k == -1)
k = i;
else if (marker_info[k].cf < marker_info[i].cf)
k = i;
}
}
if (k != -1)
{
if (!use_history_ || cont_f_ == 0)
{
arGetTransMat(&marker_info[k], marker_center_, marker_width_, marker_trans_);
}
else
{
arGetTransMatCont(&marker_info[k], marker_trans_, marker_center_, marker_width_, marker_trans_);
}
cont_f_ = 1;
// get the transformation between the marker and the real camera
double arQuat[4], arPos[3];
// arUtilMatInv (marker_trans_, cam_trans);
arUtilMat2QuatPos(marker_trans_, arQuat, arPos);
// error checking
if(fabs(sqrt(arQuat[0]*arQuat[0] + arQuat[1]*arQuat[1] + arQuat[2]*arQuat[2] + arQuat[3]*arQuat[3]) - 1.0) > 0.0001)
{
ROS_WARN("Skipping invalid frame. Computed quaternion is invalid.");
}
if(std::isnan(arQuat[0]) || std::isnan(arQuat[1]) || std::isnan(arQuat[2]) || std::isnan(arQuat[3]))
{
ROS_WARN("Skipping invalid frame because computed orientation is not a number.");
return;
}
if(std::isinf(arQuat[0]) || std::isinf(arQuat[1]) || std::isinf(arQuat[2]) || std::isinf(arQuat[3]))
{
ROS_WARN("Skipping invalid frame because computed orientation is infinite.");
return;
}
// convert to ROS frame
double quat[4], pos[3];
pos[0] = arPos[0] * AR_TO_ROS;
pos[1] = arPos[1] * AR_TO_ROS;
pos[2] = arPos[2] * AR_TO_ROS;
quat[0] = -arQuat[0];
quat[1] = -arQuat[1];
quat[2] = -arQuat[2];
quat[3] = arQuat[3];
ROS_DEBUG(" Pos x: %3.5f y: %3.5f z: %3.5f", pos[0], pos[1], pos[2]);
ROS_DEBUG(" Quat qx: %3.5f qy: %3.5f qz: %3.5f qw: %3.5f", quat[0], quat[1], quat[2], quat[3]);
// publish the marker
ar_target_marker_.confidence = marker_info->cf;
ar_marker_publisher_.publish(ar_target_marker_);
ROS_DEBUG ("Published ar_single marker");
// publish transform between camera and marker
tf::Quaternion rotation(quat[0], quat[1], quat[2], quat[3]);
tf::Vector3 origin(pos[0], pos[1], pos[2]);
tf::Transform transform(rotation, origin);
// TODO: figure out why this happens once in a while...
if(transform.getOrigin().getZ() < 0.0)
{
transform.setOrigin(-transform.getOrigin());
}
if (publish_tf_)
{
if (reverse_transform_)
{
ROS_ASSERT_MSG(false, "Reverse transform is not debugged yet.");
// tf::StampedTransform marker_to_cam(t.inverse(), image_msg->header.stamp, marker_frame_.c_str(), image_msg->header.frame_id);
tf::StampedTransform marker_to_cam(transform.inverse(), image_msg->header.stamp, marker_frame_, camera_frame_);
tf_broadcaster_.sendTransform(marker_to_cam);
}
else
{
tf::Transform offseted_transform = transform * marker_offset_;
// tf::StampedTransform cam_to_marker(t, image_msg->header.stamp, image_msg->header.frame_id, marker_frame_.c_str());
tf::StampedTransform cam_to_marker(offseted_transform, image_msg->header.stamp, camera_frame_, marker_frame_);
// tf::StampedTransform cam_to_marker(transform, image_msg->header.stamp, camera_frame_, marker_frame_);
tf_broadcaster_.sendTransform(cam_to_marker);
}
}
// publish visual marker
publishMarker(transform, image_msg->header.stamp);
}
else
{
cont_f_ = 0;
ROS_WARN("Failed to locate marker.");
}
}
void laserScan_callback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
//transform laser scan to dsf format
std::vector<float> x_scan, y_scan;
x_scan.resize(msg->ranges.size());
y_scan.resize(msg->ranges.size());
for(size_t i=0; i < msg->ranges.size(); i++)
{
x_scan[i] = msg->ranges[i] * cos(msg->angle_min + msg->angle_increment * i);
y_scan[i] = msg->ranges[i] * sin(msg->angle_min + msg->angle_increment * i);
}
std::vector<float> out = dsf_->getMostLikelyLocation(x_scan, y_scan);
//get transform from laser to base_link
float goal_x_laser = out[0] - 0.3f;
float goal_y_laser = out[1];
tf::TransformListener listener;
tf::StampedTransform transform;
try{
listener.waitForTransform("/base_link", "/base_laser_link", ros::Time(0), ros::Duration(10.0) );
listener.lookupTransform("/base_link", "/base_laser_link",
ros::Time(0), transform);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
tf::Vector3 origin = transform.getOrigin();
//transform goal to base link coordinates
tf::Vector3 goal_laser(goal_x_laser, goal_y_laser, 0);
float goal_x_base = goal_laser[0] + origin[0];
float goal_y_base = goal_laser[1];
publishMarker(goal_x_base, goal_y_base,out[2], "/base_link");
std::cout << "Relative to the robot (base_link):" << goal_x_base << " " << goal_y_base << std::endl;
//transform goal to /map coordinates
/*{
tf::TransformListener listener;
tf::StampedTransform transform;
try{
listener.waitForTransform("/map", "/base_link", ros::Time(0), ros::Duration(10.0) );
listener.lookupTransform("/map", "/base_link",
ros::Time(0), transform);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
tf::Vector3 base_goal_map_cs(goal_x_base, goal_y_base, 0);
base_goal_map_cs = transform * base_goal_map_cs;
publishMarker(base_goal_map_cs[0], base_goal_map_cs[1],out[2], "/map");
}*/
if(out[3] < object_found_threshold_)
{
return;
}
else
{
//if threshold is ok, then save goal!
base_goal_ = tf::Vector3(goal_x_base, goal_y_base, out[2]);
}
}
//only called on obstacle detected
//Relies on empty point clouds is still being published!
void pcCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
//if (activated == true)
//{
static uint8_t obstacle_state = 0;
static ros::Time last_time = ros::Time::now();
ros::Duration duration(3.0);
ros::Time cur_time = ros::Time::now();
//ROS_INFO("cur_time %.2f and last_time %.2f and duration %.2f", cur_time.toSec(), last_time.toSec(), duration.toSec());
if ((cur_time - last_time) > duration)
{
ROS_INFO("Resetting obstacle state");
obstacle_state = 0;
}
//ROS_INFO("Got point cloud detection result..");
pcl::PointCloud<pcl::PointXYZ>::Ptr input (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr output (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*msg.get(), *input.get());
// transform
pcl_ros::transformPointCloud("/base_link", *input, *output, tf_listener);
// mutex lock cloud_bb
bb_mutex.lock();
// calculate bounding box
getBoundingBox(*output, cloud_bb);
float bb_zmax = cloud_bb.z_max;
if (bb_zmax > 0)
{
publishMarker(cloud_bb);
}
bb_mutex.unlock();
if (bb_zmax > HUMAN_MIN)
{
human_detected = true;
obstacle_mutex.lock();
obstacle_detected = true;
obstacle_mutex.unlock();
if (obstacle_state != 1)
{
ROS_INFO("Obstacle detected (%.2fm): human", bb_zmax);
obstacle_state = 1;
}
}
else if (bb_zmax > OBS_MIN) // higher than the ground
{
human_detected = false;
obstacle_mutex.lock();
obstacle_detected = true;
obstacle_mutex.unlock();
if (obstacle_state != 2)
{
ROS_INFO("Obstacle detected (%.2fm): static object", bb_zmax);
obstacle_state = 2;
}
}//probably no obstacle
else
{
obstacle_mutex.lock();
obstacle_detected = false;
obstacle_mutex.unlock();
}
last_time = cur_time;
}
void ARMultiPublisher::getTransformationCallback(const sensor_msgs::ImageConstPtr & image_msg)
{
// Get the image from ROSTOPIC
// NOTE: the dataPtr format is BGR because the ARToolKit library was
// build with V4L, dataPtr format change according to the
// ARToolKit configure option (see config.h).
try
{
capture_ = bridge_.imgMsgToCv(image_msg, "bgr8");
}
catch (sensor_msgs::CvBridgeException & e)
{
ROS_ERROR ("Could not convert from >%s< to 'bgr8'.", image_msg->encoding.c_str ());
}
// cvConvertImage(capture,capture,CV_CVTIMG_FLIP);
ARUint8* data_ptr = (ARUint8 *)capture_->imageData;
// detect the markers in the video frame
if (arDetectMarker(data_ptr, threshold_, &marker_info_, &num_detected_marker_) < 0)
{
argCleanup();
ROS_BREAK ();
}
ROS_DEBUG("Detected >%i< of >%i< markers.", num_detected_marker_, num_total_markers_);
double error = 0.0;
if ((error = arMultiGetTransMat(marker_info_, num_detected_marker_, multi_marker_config_)) < 0)
{
// ROS_ERROR("Could not get transformation. This should never happen.");
ROS_WARN("Could not get transformation.");
return;
}
ROS_DEBUG("Error is >%f<.", error);
for (int i = 0; i < num_detected_marker_; i++)
{
ROS_DEBUG("multi_marker_config_->prevF: %i", multi_marker_config_->prevF);
ROS_DEBUG("%s: (%i) pos: %f %f id: %i cf: %f", marker_frame_.c_str(), i, marker_info_[i].pos[0], marker_info_[i].pos[1], marker_info_[i].id, marker_info_[i].cf);
}
// choose those with the highest confidence
std::vector<double> cfs(num_total_markers_, 0.0);
marker_indizes_.clear();
for (int i = 0; i < num_total_markers_; ++i)
{
marker_indizes_.push_back(-1);
}
for (int i = 0; i < num_total_markers_; ++i)
{
for (int j = 0; j < num_detected_marker_; j++)
{
if (!(marker_info_[j].id < 0))
{
if (marker_info_[j].cf > cfs[marker_info_[j].id])
{
cfs[marker_info_[j].id] = marker_info_[j].cf;
marker_indizes_[marker_info_[j].id] = j;
}
}
}
}
double ar_quat[4], ar_pos[3];
arUtilMat2QuatPos(multi_marker_config_->trans, ar_quat, ar_pos);
tf::Quaternion rotation(-ar_quat[0], -ar_quat[1], -ar_quat[2], ar_quat[3]);
tf::Vector3 origin(ar_pos[0] * AR_TO_ROS, ar_pos[1] * AR_TO_ROS, ar_pos[2] * AR_TO_ROS);
tf::Transform transform(rotation, origin);
if (multi_marker_config_->prevF && publish_tf_)
{
if(error < error_threshold_)
{
ROS_DEBUG("%f %f %f | %f %f %f %f | %f", origin.getX(), origin.getY(), origin.getZ(), rotation.getX(), rotation.getY(), rotation.getZ(), rotation.getW(), image_msg->header.stamp.toSec());
tf::StampedTransform cam_to_marker(transform, image_msg->header.stamp, camera_frame_, marker_frame_);
tf_broadcaster_.sendTransform(cam_to_marker);
}
publishErrorMarker(error, image_msg->header.stamp);
}
if(publish_visual_markers_)
{
for (int i = 0; i < num_total_markers_; i++)
{
if (marker_indizes_[i] >= 0)
{
tf::Transform marker_transform;
getTransform(i, marker_transform);
tf::Transform marker = transform * marker_transform;
publishMarker(i, marker, image_msg->header.stamp);
last_transform_ = marker;
}
// else
// {
// publishMarker(i, last_transform_, image_msg->header.stamp);
// }
}
}
}
/*!
* @brief Callback for point clouds.
* Callback for point clouds. Uses PCL to find the centroid
* of the points in a box in the center of the point cloud.
* Publishes cmd_vel messages with the goal from the cloud.
* @param cloud The point cloud message.
*/
void cloudcb(const PointCloud::ConstPtr& cloud)
{
//X,Y,Z of the centroid
float x = 0.0;
float y = 0.0;
float z = 1e6;
//Number of points observed
unsigned int n = 0;
bool direction = 0 ;
double rand_angular;
int count = 0;
ros::Rate loop_rate(10) ;
//Iterate through all the points in the region and find the average of the position
BOOST_FOREACH (const pcl::PointXYZ& pt, cloud->points)
{
//First, ensure that the point's position is valid. This must be done in a seperate
//if because we do not want to perform comparison on a nan value.
if (!std::isnan(x) && !std::isnan(y) && !std::isnan(z))
{
//Test to ensure the point is within the aceptable box.
if (-pt.y > min_y_ && -pt.y < max_y_ && pt.x < max_x_ && pt.x > min_x_ && pt.z < max_z_)
{
//Add the point to the totals
x += pt.x;
y += pt.y;
z = std::min(z, pt.z);
n++;
}
}
}
//If there are points, find the centroid and calculate the command goal.
//If there are no points, simply publish a stop goal.
if (n>4000)
{
x /= n;
y /= n;
if (z-goal_z_ > 0){
ROS_INFO("near goal %f %f %f with %d points", x, y, z, n);
geometry_msgs::TwistPtr cmd(new geometry_msgs::Twist());
//ROS_INFO("Don't change direction!'") ;
if(bumper_left_pressed_){
ROS_INFO("Bumper_left_ is pressing!");
while(ros::ok()&&change_direction_){
count ++;
cmd->angular.z = -0.4 ;
cmd->linear.x = -0.2 ;
cmdpub_.publish(cmd);
loop_rate.sleep();
if (count > 15){
change_direction_ = false ;
count = 0 ;
}
}
}else if(bumper_center_pressed_){
while(ros::ok()&&change_direction_){
count ++;
//ROS_INFO("I am changing!") ;
cmd->angular.z = -0.5 ;
cmd->linear.x = -0.2 ;
cmdpub_.publish(cmd);
loop_rate.sleep();
if (count > 20){
change_direction_ = false ;
count = 0 ;
}
}
}else if(bumper_right_pressed_){
while(ros::ok()&&change_direction_){
count ++;
ROS_INFO("I am changing!") ;
cmd->angular.z = 0.4 ;
cmd->linear.x = -0.2 ;
cmdpub_.publish(cmd);
loop_rate.sleep();
if (count > 15){
change_direction_ = false ;
count = 0 ;
}
}
}else{
ROS_INFO("**************");
cmd->linear.x = 0.2 ;
if(x > 0.2 ){
direction = 1 ;
srand((unsigned)time(NULL)) ;
rand_angular = rand()%7 ;
rand_angular = rand_angular/7.0 ;
ROS_INFO(" x > 0.2 , rand_angular %f ", rand_angular);
if(rand_angular > 0.7 && rand_angular <= 1.0 ){
cmd->angular.z = 0.4 ;
ROS_INFO("rand_angular = 0.7~1.0, cmd->angular.z = %f ", cmd->angular.z );
}else if(rand_angular > 0.4 && rand_angular <= 0.7){
cmd->angular.z = rand_angular ;
ROS_INFO("rand_angular = 0.4~0.7, cmd->angular.z = %f ", cmd->angular.z );
}else{
cmd->angular.z = 0.3 ;
ROS_INFO("rand_angular = 0.0~0.4, cmd->angular.z = %f ", cmd->angular.z );
}
}else if(x <- 0.2){
direction = 0 ;
srand((unsigned)time(NULL)) ;
rand_angular = rand()%7 ;
rand_angular = double(rand_angular)/7.0 - 1.0 ;
ROS_INFO(" x < -0.2 , rand_angular %f ", rand_angular);
if(rand_angular >= -1.0 && rand_angular < -0.7 ){
cmd->angular.z = -0.36 ;
ROS_INFO("rand_angular = -1.0~-0.7, cmd->angular.z = %f ", cmd->angular.z );
}else if(rand_angular >= -0.7 && rand_angular < -0.5){
cmd->angular.z = rand_angular ;
ROS_INFO("rand_angular = -0.7~-0.4, cmd->angular.z = %f ", cmd->angular.z );
}else {
cmd->angular.z = - 0.2 ;
ROS_INFO("rand_angular = -0.4~-0.0, cmd->angular.z = %f ", cmd->angular.z );
}
}else{
if(direction){
cmd->angular.z = 0.3 ;
}else{
if(!change_direction_)
cmd->angular.z = -0.3 ;
}
}
cmdpub_.publish(cmd) ;
}
}else{
ROS_INFO("goal is bingo %f %f %f with %d points", x, y, z, n);
geometry_msgs::TwistPtr cmd(new geometry_msgs::Twist());
if(bumper_left_pressed_){
ROS_INFO("Bumper_left_ is pressing!");
while(ros::ok()&&change_direction_){
count ++;
cmd->angular.z = -0.4 ;
cmd->linear.x = -0.2 ;
cmdpub_.publish(cmd);
loop_rate.sleep();
if (count > 15){
change_direction_ = false ;
count = 0 ;
}
}
}else if(bumper_center_pressed_){
while(ros::ok()&&change_direction_){
count ++;
cmd->angular.z = -0.4 ;
cmd->linear.x = -0.2 ;
cmdpub_.publish(cmd);
loop_rate.sleep();
if (count > 20){
change_direction_ = false ;
count = 0 ;
}
}
}else if(bumper_right_pressed_){
while(ros::ok()&&change_direction_){
count ++;
ROS_INFO("I am changing!") ;
cmd->angular.z = 0.4 ;
cmd->linear.x = -0.2 ;
cmdpub_.publish(cmd);
loop_rate.sleep();
if (count > 15){
change_direction_ = false ;
count = 0 ;
}
}
}else{
cmd->linear.x = 0 ;
int num = 0;
double ang = 0.0 ;
if(x > 0.2){
srand((unsigned)time(NULL));
num = (rand() % MAX);
ang = double(num) / MAX ;
ROS_INFO(" x > 0.2 , ang %f ", ang);
direction = 1 ;
if(ang > 0.7 && ang <= 1 ){
cmd->angular.z = 0.4 ;
ROS_INFO("ang=0.7~1.0, ancmd->angular.z = %f ", cmd->angular.z );
}else if(ang > 0.4 && ang <= 0.7){
cmd->angular.z = ang ;
ROS_INFO("ang=0.4~0.7, cmd->angular.z = %f ", cmd->angular.z );
}else{
cmd->angular.z = 0.3 ;
ROS_INFO("ang=0.0~0.4,cmd->angular.z = %f ", cmd->angular.z );
}
}else if(x < -0.2){
srand((unsigned)time(NULL));
num = (rand() % MAX);
ang = double(num) / MAX ;
ang = ang - 1.0 ;
direction = 0 ;
ROS_INFO(" x < -0.2 , ang %f ", ang);
if(ang >= -1.0 && ang < -0.7 ){
cmd->angular.z = -0.2 ;
ROS_INFO("ang=-1.0~-0.7, ancmd->angular.z = %f ", cmd->angular.z );
}else if(ang >= -0.7 && ang < -0.4){
cmd->angular.z = ang ;
ROS_INFO("ang=-0.7~-0.4, ancmd->angular.z = %f ", cmd->angular.z );
}else {
cmd->angular.z = - 0.2 ;
ROS_INFO("ang=-0.4~-0.0, ancmd->angular.z = %f ", cmd->angular.z );
}
}else{
ROS_INFO(" direction is %d ", direction);
if(direction){
cmd->angular.z = 0.3 ;
}else{
cmd->angular.z = -0.3 ;
}
}
cmdpub_.publish(cmd) ;
}
}
}else{
ROS_DEBUG("No points detected, stopping the robot");
publishMarker(x, y, z);
if (enabled_)
{
geometry_msgs::TwistPtr cmd(new geometry_msgs::Twist());
ROS_INFO("There are no points!x=%f , y=%f , z=%f,points=%d",x,y,z,n);
if(bumper_left_pressed_){
ROS_INFO("Bumper_left_ is pressing!");
while(ros::ok()&&change_direction_){
count ++;
//ROS_INFO("I am changing!") ;
cmd->angular.z = -0.4 ;
cmd->linear.x = -0.2 ;
cmdpub_.publish(cmd);
loop_rate.sleep();
if (count > 15){
change_direction_ = false ;
count = 0 ;
}
}
}else if(bumper_center_pressed_){
while(ros::ok()&&change_direction_){
count ++;
ROS_INFO("I am changing!") ;
cmd->angular.z = -0.5 ;
cmd->linear.x = -0.2 ;
cmdpub_.publish(cmd);
loop_rate.sleep();
if (count > 20){
change_direction_ = false ;
count = 0 ;
}
}
}else if(bumper_right_pressed_){
while(ros::ok()&&change_direction_){
count ++;
ROS_INFO("I am changing!") ;
cmd->angular.z = 0.4 ;
cmd->linear.x = -0.2 ;
cmdpub_.publish(cmd);
loop_rate.sleep();
if (count > 15){
change_direction_ = false ;
count = 0 ;
}
}
}else{
cmd->linear.x = 0.2 ;
cmdpub_.publish(cmd);
}
}
}
publishBbox();
}
