void DiffDrivePoseControllerROS::spinOnce() { if (this->getState()) { ROS_DEBUG_STREAM_THROTTLE(1.0, "Controller spinning. [" << name_ <<"]"); // determine pose difference in polar coordinates if (!getPoseDiff()) { ROS_WARN_STREAM_THROTTLE(1.0, "Getting pose difference failed. Skipping control loop. [" << name_ <<"]"); return; } // determine controller output (v, w) and check if goal is reached step(); // set control output (v, w) setControlOutput(); // Logging ROS_DEBUG_STREAM_THROTTLE(1.0, "Current state: [" << name_ <<"]"); ROS_DEBUG_STREAM_THROTTLE(1.0, "r = " << r_ << ", theta = " << theta_ << ", delta = " << delta_ << " [" << name_ <<"]"); ROS_DEBUG_STREAM_THROTTLE(1.0, "cur = " << cur_ << ", v = " << v_ << ", w = " << w_ << " [" << name_ <<"]"); } else { ROS_DEBUG_STREAM_THROTTLE(3.0, "Controller is disabled. Idling ... [" << name_ <<"]"); } }
void TrackerViewer::spin() { boost::format fmtWindowTitle ("ViSP MBT tracker viewer - [ns: %s]"); fmtWindowTitle % ros::this_node::getNamespace (); vpDisplayX d(image_, image_.getWidth(), image_.getHeight(), fmtWindowTitle.str().c_str()); vpImagePoint point (10, 10); vpImagePoint pointTime (22, 10); vpImagePoint pointCameraTopic (34, 10); ros::Rate loop_rate(80); boost::format fmt("tracking (x=%f y=%f z=%f)"); boost::format fmtTime("time = %f"); boost::format fmtCameraTopic("camera topic = %s"); fmtCameraTopic % rectifiedImageTopic_; while (!exiting()) { vpDisplay::display(image_); displayMovingEdgeSites(); displayKltPoints(); if (cMo_) { try { tracker_.initFromPose(image_, *cMo_); tracker_.display(image_, *cMo_, cameraParameters_, vpColor::red); } catch(...) { ROS_DEBUG_STREAM_THROTTLE(10, "failed to display cmo"); } ROS_DEBUG_STREAM_THROTTLE(10, "cMo viewer:\n" << *cMo_); fmt % (*cMo_)[0][3] % (*cMo_)[1][3] % (*cMo_)[2][3]; vpDisplay::displayCharString (image_, point, fmt.str().c_str(), vpColor::red); fmtTime % info_->header.stamp.toSec(); vpDisplay::displayCharString (image_, pointTime, fmtTime.str().c_str(), vpColor::red); vpDisplay::displayCharString (image_, pointCameraTopic, fmtCameraTopic.str().c_str(), vpColor::red); } else vpDisplay::displayCharString (image_, point, "tracking failed", vpColor::red); vpDisplay::flush(image_); ros::spinOnce(); loop_rate.sleep(); } }
bool DiffDrivePoseControllerROS::getPoseDiff() { // use tf to get information about the goal pose relative to the base try { tf_listener_.lookupTransform(base_frame_name_, goal_frame_name_, ros::Time(0), tf_goal_pose_rel_); } catch (tf::TransformException const &ex) { ROS_WARN_STREAM_THROTTLE(1.0, "Couldn't get transform from base to goal pose! [" << name_ <<"]"); ROS_DEBUG_STREAM_THROTTLE(1.0, "tf error: " << ex.what()); return false; } // determine distance to goal double r = std::sqrt( std::pow(tf_goal_pose_rel_.getOrigin().getX(), 2) + std::pow(tf_goal_pose_rel_.getOrigin().getY(), 2)); // determine orientation of r relative to the base frame double delta = std::atan2(-tf_goal_pose_rel_.getOrigin().getY(), tf_goal_pose_rel_.getOrigin().getX()); // determine orientation of r relative to the goal frame // helper: theta = tf's orientation + delta double heading = mtk::wrapAngle(tf::getYaw(tf_goal_pose_rel_.getRotation())); double theta = heading + delta; setInput(r, delta, theta); return true; }
void CovarianceVisual::setMessage (const geometry_msgs::PoseWithCovariance& msg) { // Construct pose position and orientation. const geometry_msgs::Point& p = msg.pose.position; Ogre::Vector3 position (p.x, p.y, p.z); Ogre::Quaternion orientation (msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z); // Set position and orientation for axes scene node. if(!position.isNaN()) axes_->setPosition (position); else ROS_WARN_STREAM_THROTTLE(1, "position contains NaN: " << position); if(!orientation.isNaN()) axes_->setOrientation (orientation); else ROS_WARN_STREAM_THROTTLE(1, "orientation contains NaN: " << orientation); // check for NaN in covariance for (unsigned i = 0; i < 3; ++i) { if(isnan(msg.covariance[i])) { ROS_WARN_THROTTLE(1, "covariance contains NaN"); return; } } // Compute eigen values and vectors for both shapes. std::pair<Eigen::Matrix3d, Eigen::Vector3d> positionEigenVectorsAndValues (computeEigenValuesAndVectors (msg, 0)); std::pair<Eigen::Matrix3d, Eigen::Vector3d> orientationEigenVectorsAndValues (computeEigenValuesAndVectors (msg, 3)); Ogre::Quaternion positionQuaternion (computeRotation (msg, positionEigenVectorsAndValues)); Ogre::Quaternion orientationQuaternion (computeRotation (msg, orientationEigenVectorsAndValues)); positionNode_->setOrientation (positionQuaternion); orientationNode_->setOrientation (orientationQuaternion); // Compute scaling. Ogre::Vector3 positionScaling (std::sqrt (positionEigenVectorsAndValues.second[0]), std::sqrt (positionEigenVectorsAndValues.second[1]), std::sqrt (positionEigenVectorsAndValues.second[2])); positionScaling *= scaleFactor_; Ogre::Vector3 orientationScaling (std::sqrt (orientationEigenVectorsAndValues.second[0]), std::sqrt (orientationEigenVectorsAndValues.second[1]), std::sqrt (orientationEigenVectorsAndValues.second[2])); orientationScaling *= scaleFactor_; // Set the scaling. if(!positionScaling.isNaN()) positionNode_->setScale (positionScaling); else ROS_WARN_STREAM("positionScaling contains NaN: " << positionScaling); if(!orientationScaling.isNaN()) orientationNode_->setScale (orientationScaling); else ROS_WARN_STREAM("orientationScaling contains NaN: " << orientationScaling); // Debugging. ROS_DEBUG_STREAM_THROTTLE (1., "Position:\n" << position << "\n" << "Positional part 3x3 eigen values:\n" << positionEigenVectorsAndValues.second << "\n" << "Positional part 3x3 eigen vectors:\n" << positionEigenVectorsAndValues.first << "\n" << "Sphere orientation:\n" << positionQuaternion << "\n" << positionQuaternion.getRoll () << " " << positionQuaternion.getPitch () << " " << positionQuaternion.getYaw () << "\n" << "Sphere scaling:\n" << positionScaling << "\n" << "Rotational part 3x3 eigen values:\n" << orientationEigenVectorsAndValues.second << "\n" << "Rotational part 3x3 eigen vectors:\n" << orientationEigenVectorsAndValues.first << "\n" << "Cone orientation:\n" << orientationQuaternion << "\n" << orientationQuaternion.getRoll () << " " << orientationQuaternion.getPitch () << " " << orientationQuaternion.getYaw () << "\n" << "Cone scaling:\n" << orientationScaling ); }