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(); }