static void mouseCb(int event, int x, int y, int flags, void* param)
{
if (event == cv::EVENT_LBUTTONDOWN) {
ROS_WARN_ONCE("Left-clicking no longer saves images. Right-click instead.");
return;
} else if (event != cv::EVENT_RBUTTONDOWN) {
return;
}
boost::mutex::scoped_lock lock(g_image_mutex);
const cv::Mat &image = g_last_image;
if (image.empty()) {
ROS_WARN("Couldn't save image, no data!");
return;
}
std::string filename = (g_filename_format % g_count).str();
if (cv::imwrite(filename, image)) {
ROS_INFO("Saved image %s", filename.c_str());
g_count++;
} else {
boost::filesystem::path full_path = boost::filesystem::complete(filename);
ROS_ERROR_STREAM("Failed to save image. Have permission to write there?: " << full_path);
}
}
sensor_msgs::CameraInfoPtr AstraDriver::getIRCameraInfo(int width, int height, ros::Time time) const
{
sensor_msgs::CameraInfoPtr info;
if (ir_info_manager_->isCalibrated())
{
info = boost::make_shared<sensor_msgs::CameraInfo>(ir_info_manager_->getCameraInfo());
if ( info->width != width )
{
// Use uncalibrated values
ROS_WARN_ONCE("Image resolution doesn't match calibration of the IR camera. Using default parameters.");
info = getDefaultCameraInfo(width, height, device_->getIRFocalLength(height));
}
}
else
{
// If uncalibrated, fill in default values
info = getDefaultCameraInfo(width, height, device_->getDepthFocalLength(height));
}
// Fill in header
info->header.stamp = time;
info->header.frame_id = depth_frame_id_;
return info;
}
void OccupancyGrid::visualize()
{
ROS_WARN_ONCE("[grid] Visualizing the grid is disabled. (DistanceField API changed...)");
btTransform trans;
trans.setIdentity();
grid_->visualize(0.01, 0.02, reference_frame_, trans, ros::Time::now());
}
VisionNode::VisionNode() {
cv::FileStorage fs("/home/roboy/workspace/mocap/src/intrinsics.xml", cv::FileStorage::READ);
if (!fs.isOpened()) {
ROS_ERROR("could not open intrinsics.xml");
return;
}
fs["camera_matrix"] >> cameraMatrix;
fs["distortion_coefficients"] >> distCoeffs;
fs.release();
ID = 0;
// calculate undistortion mapping
initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(),
getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, cv::Size(WIDTH, HEIGHT), 1,
cv::Size(WIDTH, HEIGHT), 0),
cv::Size(WIDTH, HEIGHT), CV_16SC2, map1, map2);
marker_position_pub = new ros::Publisher;
*marker_position_pub = nh.advertise<communication::MarkerPosition>("/mocap/marker_position", 100);
video_pub = new ros::Publisher;
*video_pub = nh.advertise<sensor_msgs::Image>("/mocap/video", 1);
camera_control_sub = nh.subscribe("/mocap/camera_control", 100, &VisionNode::camera_control, this);
cameraID_pub = new ros::Publisher;
*cameraID_pub = nh.advertise<std_msgs::Int32>("/mocap/cameraID", 100);
// Publish the marker
while (cameraID_pub->getNumSubscribers() < 1) {
ros::Duration d(1.0);
if (!ros::ok()) {
return;
}
ROS_WARN_ONCE("Waiting for mocap plugin to subscribe to /mocap/cameraID");
d.sleep();
}
ROS_INFO_ONCE("Found subscriber");
spinner = new ros::AsyncSpinner(1);
spinner->start();
std_msgs::Int32 msg;
msg.data = ID;
cameraID_pub->publish(msg);
img = cv::Mat(HEIGHT, WIDTH, CV_8UC4, img_data);
img_rectified = cv::Mat(HEIGHT, WIDTH, CV_8UC4, img_rectified_data);
t1 = std::chrono::high_resolution_clock::now();
StartCamera(WIDTH, HEIGHT, 90, CameraCallback);
}
void UAVTracker::callback(const sensor_msgs::Image::ConstPtr &image_msg) {
cv_bridge::CvImagePtr cv_ptr;
try {
cv_ptr = cv_bridge::toCvCopy(
image_msg, sensor_msgs::image_encodings::BGR8);
} catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
if (!object_init_) {
ROS_WARN_ONCE("PLEASE INITIALIZE THE OBJECT");
return;
}
cv::Mat image = cv_ptr->image;
cv::Point2f init_tl = cv::Point2f(this->screen_rect_.x,
this->screen_rect_.y);
cv::Point2f init_br = cv::Point2f(init_tl.x + this->screen_rect_.width,
init_tl.y + this->screen_rect_.height);
cv::Mat im_gray;
cv::Mat img = image.clone();
cv::cvtColor(image, im_gray, CV_RGB2GRAY);
if (!tracker_init_) {
this->initialise(im_gray, init_tl, init_br);
this->tracker_init_ = true;
}
this->processFrame(im_gray);
for (int i = 0; i < this->trackedKeypoints.size(); i++) {
cv::circle(img, this->trackedKeypoints[i].first.pt,
3, cv::Scalar(255, 255, 255));
}
cv::Scalar color = cv::Scalar(0, 255, 0);
cv::line(img, this->topLeft, this->topRight, color, 3);
cv::line(img, this->topRight, this->bottomRight, color, 3);
cv::line(img, this->bottomRight, this->bottomLeft, color, 3);
cv::line(img, this->bottomLeft, this->topLeft, color, 3);
jsk_recognition_msgs::Rect jsk_rect;
jsk_rect.x = this->topLeft.x;
jsk_rect.y = this->topLeft.y;
jsk_rect.width = (this->bottomRight.x - jsk_rect.x);
jsk_rect.height = (this->bottomRight.y - jsk_rect.y);
this->pub_rect_.publish(jsk_rect);
cv_bridge::CvImagePtr pub_msg(new cv_bridge::CvImage);
pub_msg->header = image_msg->header;
pub_msg->encoding = sensor_msgs::image_encodings::BGR8;
pub_msg->image = img.clone();
this->pub_image_.publish(pub_msg);
// cv::imshow("image", img);
// cv::waitKey(3);
}
void RosThread::joyCb(const sensor_msgs::JoyConstPtr joy_msg)
{
joyCount++;
if (joy_msg->axes.size() < 4) {
ROS_WARN_ONCE("Error: Non-compatible Joystick!");
return;
}
// Avoid crashes if non-ps3 joystick is being used
short unsigned int actiavte_index = (joy_msg->buttons.size() > 11) ? 11 : 1;
// if not controlling: start controlling if sth. is pressed (!)
bool justStartedControlling = false;
if(gui->currentControlSource != CONTROL_JOY)
{
if( joy_msg->axes[0] > 0.1 || joy_msg->axes[0] < -0.1 ||
joy_msg->axes[1] > 0.1 || joy_msg->axes[1] < -0.1 ||
joy_msg->axes[2] > 0.1 || joy_msg->axes[2] < -0.1 ||
joy_msg->axes[3] > 0.1 || joy_msg->axes[3] < -0.1 ||
joy_msg->buttons.at(actiavte_index))
{
gui->setControlSource(CONTROL_JOY);
justStartedControlling = true;
}
}
// are we actually controlling with the Joystick?
if(justStartedControlling || gui->currentControlSource == CONTROL_JOY)
{
ControlCommand c;
c.yaw = -joy_msg->axes[2];
c.gaz = joy_msg->axes[3];
c.roll = -joy_msg->axes[0];
c.pitch = -joy_msg->axes[1];
sendControlToDrone(c);
lastJoyControlSent = c;
if(!lastL1Pressed && joy_msg->buttons.at(actiavte_index - 1))
sendTakeoff();
if(lastL1Pressed && !joy_msg->buttons.at(actiavte_index - 1))
sendLand();
if(!lastR1Pressed && joy_msg->buttons.at(actiavte_index))
sendToggleState();
}
lastL1Pressed =joy_msg->buttons.at(actiavte_index - 1);
lastR1Pressed = joy_msg->buttons.at(actiavte_index);
}
bool Move::handshake(){
int counter = 0;
while(ros::ok()){
syncboard.flush();
syncboard.write("v");
std::string result = syncboard.readline();
if (result.length() > 0){
ROS_INFO("Connected to syncboard.");
return true;
}
if(counter++ > 50){
ROS_WARN_ONCE("Connecting to syncboard is taking longer than expected.");
}
ros::Rate(10).sleep();
}
ROS_WARN("Syncboard handshake failed.");
return false;
}
/**
* Constructor of the run class for toaster_vizualiser
*/
Run() {
name_list = std::vector<std::string>();
id_cpt = 1;
ros::NodeHandle reception_node;
ros::NodeHandle emission_node;
area_list = visualization_msgs::MarkerArray();
obj_list = visualization_msgs::MarkerArray();
human_list = visualization_msgs::MarkerArray();
robot_list = visualization_msgs::MarkerArray();
//definition of subscribers
sub_objList = reception_node.subscribe("/pdg/objectList", 1000, &Run::chatterCallbackObjList, this);
sub_areaList = reception_node.subscribe("/area_manager/areaList", 1000, &Run::chatterCallbackAreaList, this);
sub_humanList = reception_node.subscribe("/pdg/humanList", 1000, &Run::chatterCallbackHumanList, this);
sub_robotList = reception_node.subscribe("/pdg/robotList", 1000, &Run::chatterCallbackRobotList, this);
//definition of publishers
pub_obj = emission_node.advertise<visualization_msgs::MarkerArray>("/toaster_visualizer/marker_object", 1000);
pub_area = emission_node.advertise<visualization_msgs::MarkerArray>("/toaster_visualizer/marker_area", 1000);
pub_human = emission_node.advertise<visualization_msgs::MarkerArray>("/toaster_visualizer/marker_human", 1000);
pub_robot = emission_node.advertise<visualization_msgs::MarkerArray>("/toaster_visualizer/marker_robot", 1000);
//open xml files
// Objects
std::stringstream pathObj;
pathObj << ros::package::getPath("toaster_visualizer") << "/src/list_obj.xml";
listObj = TiXmlDocument(pathObj.str());
if (!listObj.LoadFile()) {
ROS_WARN_ONCE("Erreur lors du chargement du fichier xml");
ROS_WARN_ONCE("error # %d", listObj.ErrorId());
ROS_WARN_ONCE("%s", listObj.ErrorDesc());
}
// Humans
std::stringstream pathHuman;
pathHuman << ros::package::getPath("toaster_visualizer") << "/src/list_human_morse_joints.xml";
listMemb = TiXmlDocument(pathHuman.str());
if (!listMemb.LoadFile()) {
ROS_WARN_ONCE("Erreur lors du chargement du fichier xml");
ROS_WARN_ONCE("error # %d", listMemb.ErrorId());
ROS_WARN_ONCE("%s", listMemb.ErrorDesc());
}
// Robots
/*path.flush();
path << ros::package::getPath("toaster_visualizer") << "/src/list_robots.xml";
listRob = TiXmlDocument(path.str());
if (!listRob.LoadFile()) {
ROS_WARN_ONCE("Erreur lors du chargement du fichier xml");
ROS_WARN_ONCE("error # %d", listRob.ErrorId());
ROS_WARN_ONCE("%s", listRob.ErrorDesc());
}*/
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "IMU_reader");
ros::NodeHandle n;
ros::Rate loop_rate(80);
ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);
uint32_t shape = visualization_msgs::Marker::CUBE;
try
{
//Open serial port
ROS_INFO("Opening serial connection.");
SerialPort serial_port ("/dev/ttyACM0");
serial_port.Open(SerialPort::BAUD_9600,
SerialPort::CHAR_SIZE_8,
SerialPort::PARITY_NONE,
SerialPort::STOP_BITS_1,
SerialPort::FLOW_CONTROL_NONE);
// Enter the main loop
while (ros::ok())
{
// Wait until the data is available
if(serial_port.IsDataAvailable())
{
// Vector of strings
std::vector<std::string> strs;
// Read the serial port and pass the result to the splitting function
boost::split(strs, serial_port.ReadLine(), boost::is_any_of(","));
// 3 strings are expected, any other case is an error
if(strs.size() == 3)
{
// Print the result to the console
ROS_INFO("%s %s %s", strs[0].c_str(), strs[1].c_str(), strs[2].c_str());
// Create a marker to visualize the IMU data
visualization_msgs::Marker marker;
marker.header.frame_id = "/my_frame";
marker.header.stamp = ros::Time::now();
marker.ns = "basic_shapes";
marker.id = 0;
marker.type = shape;
marker.action = visualization_msgs::Marker::ADD;
// Convert the Roll Pitch Yaw to quaternion. BNO055 can actually output quaternions, need to look into that.
float roll = PI*atof(strs[1].c_str())/180.0;
float pitch = -PI*atof(strs[2].c_str())/180.0;
float yaw = -PI*atof(strs[0].c_str())/180.0;
//ROS_INFO("%f %f %f", roll, pitch, yaw);
tf::Quaternion quat(roll, pitch, yaw);
quat.setRPY(roll, pitch, yaw);
// Position of the marker
marker.pose.position.x = 0;
marker.pose.position.y = 0;
marker.pose.position.z = 0;
// Pose of the marker
marker.pose.orientation.x = quat.x();
marker.pose.orientation.y = quat.y();
marker.pose.orientation.z = quat.z();
marker.pose.orientation.w = quat.w();
// Scale of the marker
marker.scale.x = 3.0;
marker.scale.y = 3.0;
marker.scale.z = 0.5;
// Set the color -- be sure to set alpha to something non-zero!
marker.color.r = 0.0f;
marker.color.g = 1.0f;
marker.color.b = 0.0f;
marker.color.a = 1.0;
marker.lifetime = ros::Duration();
while (marker_pub.getNumSubscribers() < 1)
{
if (!ros::ok())
{
return 0;
}
ROS_WARN_ONCE("Please create a subscriber to the marker");
sleep(1);
}
marker_pub.publish(marker);
}
}
ros::spinOnce();
loop_rate.sleep();
}
// Close connection
if(serial_port.IsOpen())
{
ROS_INFO("Closing serial connection.");
serial_port.Close();
}
ROS_INFO("Serial node closed.");
return 0;
}
catch (SerialPort::OpenFailed exception)
{
ROS_ERROR("Failed to open the port");
}
catch (SerialPort::AlreadyOpen exception)
{
ROS_ERROR("Port is already open");
}
}
/*!
* \brief Publishes the current joint angles.
*
* Joint angles are published in both their raw state as obtained from the arm
* (JointAngles), and transformed & converted to radians (joint_state) as per
* the Kinova Kinematics PDF.
*
* Velocities and torques (effort) are only published in the JointStates
* message, only for the first 6 joints as these values are not available for
* the fingers.
*/
void KinovaArm::publishJointAngles(void)
{
FingerAngles fingers;
kinova_comm_.getFingerPositions(fingers);
if (arm_joint_number_ != 4 && arm_joint_number_ != 6 && arm_joint_number_ != 7)
{
ROS_WARN_ONCE("The joint_state publisher only supports 4, 6 and 7 DOF for now.: %d", arm_joint_number_);
}
// Query arm for current joint angles
KinovaAngles current_angles;
kinova_comm_.getJointAngles(current_angles);
kinova_msgs::JointAngles kinova_angles = current_angles.constructAnglesMsg();
AngularPosition joint_command;
kinova_comm_.getAngularCommand(joint_command);
kinova_msgs::JointAngles joint_command_msg = KinovaAngles(joint_command.Actuators).constructAnglesMsg();
sensor_msgs::JointState joint_state;
joint_state.name = joint_names_;
joint_state.header.stamp = ros::Time::now();
// Transform from Kinova DH algorithm to physical angles in radians, then place into vector array
joint_state.position.resize(joint_total_number_);
joint_state.position[0] = kinova_angles.joint1 * M_PI/180;
joint_state.position[1] = kinova_angles.joint2 * M_PI/180;
joint_state.position[2] = kinova_angles.joint3 * M_PI/180;
joint_state.position[3] = kinova_angles.joint4 * M_PI/180;
if (arm_joint_number_ >= 6)
{
joint_state.position[4] = kinova_angles.joint5 * M_PI/180;
joint_state.position[5] = kinova_angles.joint6 * M_PI/180;
}
if (arm_joint_number_ == 7)
{
joint_state.position[6] = kinova_angles.joint7 * M_PI/180;
}
if(finger_number_==2)
{
joint_state.position[joint_total_number_-2] = fingers.Finger1/6800*80*M_PI/180;
joint_state.position[joint_total_number_-1] = fingers.Finger2/6800*80*M_PI/180;
}
else if(finger_number_==3)
{
joint_state.position[joint_total_number_-3] = fingers.Finger1/6800*80*M_PI/180;
joint_state.position[joint_total_number_-2] = fingers.Finger2/6800*80*M_PI/180;
joint_state.position[joint_total_number_-1] = fingers.Finger3/6800*80*M_PI/180;
}
// Joint velocities
KinovaAngles current_vels;
kinova_comm_.getJointVelocities(current_vels);
joint_state.velocity.resize(joint_total_number_);
joint_state.velocity[0] = current_vels.Actuator1;
joint_state.velocity[1] = current_vels.Actuator2;
joint_state.velocity[2] = current_vels.Actuator3;
joint_state.velocity[3] = current_vels.Actuator4;
// no velocity info for fingers
if(finger_number_==2)
{
joint_state.velocity[joint_total_number_-2] = 0;
joint_state.velocity[joint_total_number_-1] = 0;
}
else if(finger_number_==3)
{
joint_state.velocity[joint_total_number_-3] = 0;
joint_state.velocity[joint_total_number_-2] = 0;
joint_state.velocity[joint_total_number_-1] = 0;
}
if (arm_joint_number_ >= 6)
{
joint_state.velocity[4] = current_vels.Actuator5;
joint_state.velocity[5] = current_vels.Actuator6;
}
if (arm_joint_number_ == 7)
{
joint_state.velocity[6] = current_vels.Actuator7;
}
// ROS_DEBUG_THROTTLE(0.1,
// "Raw joint velocities: %f %f %f %f %f %f",
// current_vels.Actuator1,
// current_vels.Actuator2,
// current_vels.Actuator3,
// current_vels.Actuator4,
// current_vels.Actuator5,
// current_vels.Actuator6);
if (convert_joint_velocities_) {
convertKinDeg(joint_state.velocity);
}
// Joint torques (effort)
KinovaAngles joint_tqs;
bool gravity_comp;
node_handle_.param("torque_parameters/publish_torque_with_gravity_compensation", gravity_comp, false);
if (gravity_comp==true)
kinova_comm_.getGravityCompensatedTorques(joint_tqs);
else
kinova_comm_.getJointTorques(joint_tqs);
joint_torque_publisher_.publish(joint_tqs.constructAnglesMsg());
joint_state.effort.resize(joint_total_number_);
joint_state.effort[0] = joint_tqs.Actuator1;
joint_state.effort[1] = joint_tqs.Actuator2;
joint_state.effort[2] = joint_tqs.Actuator3;
joint_state.effort[3] = joint_tqs.Actuator4;
// no effort info for fingers
if(finger_number_==2)
{
joint_state.effort[joint_total_number_-2] = 0;
joint_state.effort[joint_total_number_-1] = 0;
}
else if(finger_number_==3)
{
joint_state.effort[joint_total_number_-3] = 0;
joint_state.effort[joint_total_number_-2] = 0;
joint_state.effort[joint_total_number_-1] = 0;
}
if (arm_joint_number_ >= 6)
{
joint_state.effort[4] = joint_tqs.Actuator5;
joint_state.effort[5] = joint_tqs.Actuator6;
}
if (arm_joint_number_ == 7)
{
joint_state.effort[6] = joint_tqs.Actuator7;
}
joint_angles_publisher_.publish(kinova_angles);
joint_command_publisher_.publish(joint_command_msg);
joint_state_publisher_.publish(joint_state);
}
int main( int argc, char** argv )
{
ros::init(argc, argv, "head");
ros::NodeHandle n;
ros::Rate r(30.0);
ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);
// Set our initial shape types
uint32_t joint_shape = visualization_msgs::Marker::SPHERE;
// May be a more listener
tf::TransformListener listener;
while (ros::ok())
{
// Listen another tf
tf::StampedTransform neck_head_transform;
try{
// Essential for lookup target frame
listener.waitForTransform("tracker/neck", "tracker/head", ros::Time(0), ros::Duration(1.0));
listener.lookupTransform("tracker/neck", "tracker/head", ros::Time(0), neck_head_transform);
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
visualization_msgs::Marker head, neck;
// Set the frame ID and timestamp. See the TF tutorials for information on these.
head.header.frame_id = "tracker/head";
neck.header.frame_id = "tracker/neck";
//hand.header.frame_id = "sub_skeleton_frame";
head.header.stamp = neck.header.stamp = ros::Time(0);
// Set the namespace and id for this marker. This serves to create a unique ID
// Any marker sent with the same namespace and id will overwrite the old one
head.ns = neck.ns = "head_model";
head.id = 0;
neck.id = 1;
// Set the marker type.
head.type = joint_shape;
neck.type = visualization_msgs::Marker::ARROW;
// Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
head.action = neck.action = visualization_msgs::Marker::ADD;
// Now for human model define !!!
// Hands
head.pose.orientation.w = 1.0;
head.scale.x = 0.2f;
head.scale.y = 0.2f;
head.scale.z = 0.2f;
// Neck
neck.points.resize(2);
neck.points[0].x = 0.0f;
neck.points[0].y = 0.0f;
neck.points[0].z = 0.0f;
neck.points[1].x = neck_head_transform.getOrigin().x();
neck.points[1].y = neck_head_transform.getOrigin().y();
neck.points[1].z = neck_head_transform.getOrigin().z();
neck.scale.x = 0.1f;
neck.scale.y = 0.1f;
neck.scale.z = 0.001f;
// Set the color -- be sure to set alpha to something non-zero!
head.color.r = neck.color.r = 1.0f;
head.color.a = neck.color.a = 1.0f;
// Publish the marker
while (marker_pub.getNumSubscribers() < 1)
{
if (!ros::ok())
{
return 0;
}
ROS_WARN_ONCE("Please create a subscriber to the markers");
sleep(1);
}
marker_pub.publish(head);
marker_pub.publish(neck);
r.sleep();
}
}
// %Tag(INIT)%
int main( int argc, char** argv )
{
ros::init(argc, argv, "basic_shapes");
ros::NodeHandle n;
ros::Rate r(1);
ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);
// %EndTag(INIT)%
// Set our initial shape type to be a cube
// %Tag(SHAPE_INIT)%
uint32_t shape = visualization_msgs::Marker::CUBE;
// %EndTag(SHAPE_INIT)%
// %Tag(MARKER_INIT)%
while (ros::ok())
{
visualization_msgs::Marker marker;
// Set the frame ID and timestamp. See the TF tutorials for information on these.
marker.header.frame_id = "/my_frame";
marker.header.stamp = ros::Time::now();
// %EndTag(MARKER_INIT)%
// Set the namespace and id for this marker. This serves to create a unique ID
// Any marker sent with the same namespace and id will overwrite the old one
// %Tag(NS_ID)%
marker.ns = "basic_shapes";
marker.id = 0;
// %EndTag(NS_ID)%
// Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
// %Tag(TYPE)%
marker.type = shape;
// %EndTag(TYPE)%
// Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
// %Tag(ACTION)%
marker.action = visualization_msgs::Marker::ADD;
// %EndTag(ACTION)%
// Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header
// %Tag(POSE)%
marker.pose.position.x = 0;
marker.pose.position.y = 0;
marker.pose.position.z = 0;
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
// %EndTag(POSE)%
// Set the scale of the marker -- 1x1x1 here means 1m on a side
// %Tag(SCALE)%
marker.scale.x = 1.0;
marker.scale.y = 1.0;
marker.scale.z = 1.0;
// %EndTag(SCALE)%
// Set the color -- be sure to set alpha to something non-zero!
// %Tag(COLOR)%
marker.color.r = 0.0f;
marker.color.g = 1.0f;
marker.color.b = 0.0f;
marker.color.a = 1.0;
// %EndTag(COLOR)%
// %Tag(LIFETIME)%
marker.lifetime = ros::Duration();
// %EndTag(LIFETIME)%
// Publish the marker
// %Tag(PUBLISH)%
while (marker_pub.getNumSubscribers() < 1)
{
if (!ros::ok())
{
return 0;
}
ROS_WARN_ONCE("Please create a subscriber to the marker");
sleep(1);
}
marker_pub.publish(marker);
// %EndTag(PUBLISH)%
// Cycle between different shapes
// %Tag(CYCLE_SHAPES)%
switch (shape)
{
case visualization_msgs::Marker::CUBE:
shape = visualization_msgs::Marker::SPHERE;
break;
case visualization_msgs::Marker::SPHERE:
shape = visualization_msgs::Marker::ARROW;
break;
case visualization_msgs::Marker::ARROW:
shape = visualization_msgs::Marker::CYLINDER;
break;
case visualization_msgs::Marker::CYLINDER:
shape = visualization_msgs::Marker::CUBE;
break;
}
// %EndTag(CYCLE_SHAPES)%
// %Tag(SLEEP_END)%
r.sleep();
}
// %EndTag(SLEEP_END)%
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "basic_shapes");
ros::NodeHandle n;
ros::Rate r(10);
ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);
uint32_t shape = visualization_msgs::Marker::MESH_RESOURCE;
while (ros::ok()) {
visualization_msgs::Marker marker;
marker.header.frame_id = "base_footprint";
marker.header.stamp = ros::Time::now();
marker.ns = "basic_shapes";
marker.id = 0;
marker.type = shape;
marker.mesh_resource = "package://using_markers/meshes/TornilloM5x35.stl";
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = 0;
marker.pose.position.y = 0;
marker.pose.position.z = 0;
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
marker.scale.x = 0.1;
marker.scale.y = 0.1;
marker.scale.z = 0.1;
marker.color.r = 0.0f;
marker.color.g = 1.0f;
marker.color.b = 1.0f;
marker.color.a = 1.0f;
marker.lifetime = ros::Duration();
while (marker_pub.getNumSubscribers() < 1) {
if (!ros::ok()) {
return 0;
}
ROS_WARN_ONCE("Please create a subscriber to the marker");
}
marker_pub.publish(marker);
/*
switch (shape) {
case visualization_msgs::Marker::CUBE:
shape = visualization_msgs::Marker::SPHERE;
break;
case visualization_msgs::Marker::SPHERE:
shape = visualization_msgs::Marker::ARROW;
break;
case visualization_msgs::Marker::ARROW:
shape = visualization_msgs::Marker::CYLINDER;
break;
case visualization_msgs::Marker::CYLINDER:
shape = visualization_msgs::Marker::CUBE;
break;
}
*/
r.sleep();
}
}
int main( int argc, char** argv )
{
ros::init(argc, argv, "left_fore_leg");
ros::NodeHandle n;
ros::Rate r(30.0);
ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);
// Set our initial shape types
uint32_t limb_shape = visualization_msgs::Marker::ARROW;
uint32_t joint_shape = visualization_msgs::Marker::SPHERE;
// May be a more listener
tf::TransformListener listener;
while (ros::ok())
{
// Listen another tf
tf::StampedTransform knee_foot_transform;
try{
// Essential for lookup target frame
listener.waitForTransform("tracker/left_knee", "tracker/left_foot", ros::Time(0), ros::Duration(1.0));
listener.lookupTransform("tracker/left_knee", "tracker/left_foot", ros::Time(0), knee_foot_transform);
//listener.waitForTransform("skeleton_frame", "sub_skeleton_frame", ros::Time(0), ros::Duration(1.0));
//listener.lookupTransform("skeleton_frame", "sub_skeleton_frame", ros::Time(0), knee_foot_transform);
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
visualization_msgs::Marker knee, knee_foot;
// Set the frame ID and timestamp. See the TF tutorials for information on these.
knee.header.frame_id = knee_foot.header.frame_id = "tracker/left_knee";
//knee.header.frame_id = knee_foot.header.frame_id = "skeleton_frame";
knee.header.stamp = knee_foot.header.stamp = ros::Time(0);
// Set the namespace and id for this marker. This serves to create a unique ID
// Any marker sent with the same namespace and id will overwrite the old one
knee.ns = knee_foot.ns = "left_fore_leg_model";
knee.id = 0;
knee_foot.id = 1;
// Set the marker type.
knee_foot.type = limb_shape;
knee.type = joint_shape;
// Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
knee.action = knee_foot.action = visualization_msgs::Marker::ADD;
// Now for human model define !!!
// Elbow
knee.pose.orientation.w = 1.0;
knee.scale.x = 0.1f;
knee.scale.y = 0.1f;
knee.scale.z = 0.1f;
// Elbow_foot
knee_foot.points.resize(2);
knee_foot.points[0].x = 0.0f;
knee_foot.points[0].y = 0.0f;
knee_foot.points[0].z = 0.0f;
//ROS_INFO("We got knee_foot [%f]", knee_foot_transform.getOrigin().x());
knee_foot.points[1].x = knee_foot_transform.getOrigin().x();
knee_foot.points[1].y = knee_foot_transform.getOrigin().y();
knee_foot.points[1].z = knee_foot_transform.getOrigin().z();
knee_foot.scale.x = 0.1f;
knee_foot.scale.y = 0.1f;
knee_foot.scale.z = 0.001f;
// Set the color -- be sure to set alpha to something non-zero!
knee.color.r = knee_foot.color.r = 1.0f;
knee.color.a = knee_foot.color.a = 1.0f;
//knee.lifetime = knee_foot.lifetime = ros::Duration();
// Publish the marker
while (marker_pub.getNumSubscribers() < 1)
{
if (!ros::ok())
{
return 0;
}
ROS_WARN_ONCE("Please create a subscriber to the markers");
sleep(1);
}
marker_pub.publish(knee);
marker_pub.publish(knee_foot);
r.sleep();
}
}
int main( int argc, char** argv )
{
ros::init(argc, argv, "right_upper_arm");
ros::NodeHandle n;
ros::Rate r(30.0);
ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);
// Set our initial shape types
uint32_t limb_shape = visualization_msgs::Marker::ARROW;
uint32_t joint_shape = visualization_msgs::Marker::SPHERE;
// May be a more listener
tf::TransformListener listener;
while (ros::ok())
{
// Listen another tf
tf::StampedTransform shoulder_elbow_transform;
try{
// Essential for lookup target frame
listener.waitForTransform("tracker/right_shoulder", "tracker/right_elbow", ros::Time(0), ros::Duration(1.0));
listener.lookupTransform("tracker/right_shoulder", "tracker/right_elbow", ros::Time(0), shoulder_elbow_transform);
//listener.waitForTransform("camera_frame", "skeleton_frame", ros::Time(0), ros::Duration(1.0));
//listener.lookupTransform("camera_frame", "skeleton_frame", ros::Time(0), shoulder_elbow_transform);
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
visualization_msgs::Marker shoulder, shoulder_elbow;
// Set the frame ID and timestamp. See the TF tutorials for information on these.
shoulder.header.frame_id = shoulder_elbow.header.frame_id = "tracker/right_shoulder";
//shoulder.header.frame_id = shoulder_elbow.header.frame_id = "camera_frame";
shoulder.header.stamp = shoulder_elbow.header.stamp = ros::Time(0);
// Set the namespace and id for this marker. This serves to create a unique ID
// Any marker sent with the same namespace and id will overwrite the old one
shoulder.ns = shoulder_elbow.ns = "right_upper_arm_model";
shoulder.id = 0;
shoulder_elbow.id = 1;
// Set the marker type.
shoulder_elbow.type = limb_shape;
shoulder.type = joint_shape;
// Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
shoulder.action = shoulder_elbow.action = visualization_msgs::Marker::ADD;
// Now for human model define !!!
// Shoulder
shoulder.pose.orientation.w = 1.0;
// Set the scale of the marker -- 1x1x1 here means 1m on a sider
shoulder.scale.x = 0.1f;
shoulder.scale.y = 0.1f;
shoulder.scale.z = 0.1f;
// Shoulder_elbow
// Index 0 is start point
shoulder_elbow.points.resize(2);
shoulder_elbow.points[0].x = 0.0f;
shoulder_elbow.points[0].y = 0.0f;
shoulder_elbow.points[0].z = 0.0f;
ROS_INFO("We got shoulder_elbow [%f]", shoulder_elbow_transform.getOrigin().x());
shoulder_elbow.points[1].x = shoulder_elbow_transform.getOrigin().x();
shoulder_elbow.points[1].y = shoulder_elbow_transform.getOrigin().y();
shoulder_elbow.points[1].z = shoulder_elbow_transform.getOrigin().z();
shoulder_elbow.scale.x = 0.1f;
shoulder_elbow.scale.y = 0.1f;
shoulder_elbow.scale.z = 0.001f;
//marker.scale.z = sqrt(pow(ls_transform.getOrigin().x(), 2) + pow(ls_transform.getOrigin().y(), 2) + pow(ls_transform.getOrigin().z(), 2));
// Set the color -- be sure to set alpha to something non-zero!
shoulder.color.r = shoulder_elbow.color.r = 1.0f;
shoulder.color.a = shoulder_elbow.color.a = 1.0f;
shoulder.lifetime = shoulder_elbow.lifetime = ros::Duration();
// Publish the marker
while (marker_pub.getNumSubscribers() < 1)
{
if (!ros::ok())
{
return 0;
}
ROS_WARN_ONCE("Please create a subscriber to the markers");
sleep(1);
}
marker_pub.publish(shoulder);
marker_pub.publish(shoulder_elbow);
r.sleep();
}
}
void pub_object_marker(ros::Publisher &marker_pub, ros::NodeHandle &n){
visualization_msgs::Marker marker;
// uint32_t shape = visualization_msgs::Marker::CUBE;
// uint32_t shape = visualization_msgs::Marker::SPHERE;
uint32_t shape = visualization_msgs::Marker::CUBE;
// Set the frame ID and timestamp. See the TF tutorials for information on these.
//marker.header.frame_id = "/my_frame";
marker.header.frame_id = "/base_link";
marker.header.stamp = ros::Time::now();
// Set the namespace and id for this marker. This serves to create a unique ID
// Any marker sent with the same namespace and id will overwrite the old one
marker.ns = "basic_shapes";
marker.id = 100;//rosbag_marker->id;
// Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
marker.type = shape; //rosbag_marker->type;
// Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
marker.action = visualization_msgs::Marker::ADD;
ros::ServiceClient gms_c = n.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
gazebo_msgs::GetModelState getmodelstate;
getmodelstate.request.model_name = "block";
gms_c.call(getmodelstate);
double x = getmodelstate.response.pose.position.x;
double y = getmodelstate.response.pose.position.y;
double z = getmodelstate.response.pose.position.z;
double q_x = getmodelstate.response.pose.orientation.x;
double q_y = getmodelstate.response.pose.orientation.y;
double q_z = getmodelstate.response.pose.orientation.z;
double q_w = getmodelstate.response.pose.orientation.w;
double z_offset = 0.000575;
std::cout << x << std::endl;
std::cout << y << std::endl;
std::cout << z - z_offset << std::endl;
marker.pose.position.x = x;//rosbag_marker->pose.position.x;//0;
marker.pose.position.y = y;//rosbag_marker->pose.position.y;//0;
marker.pose.position.z = z - z_offset;//rosbag_marker->pose.position.z;//0;
marker.pose.orientation.x = q_x;//rosbag_marker->pose.orientation.x;//0.0;
marker.pose.orientation.y = q_y;//rosbag_marker->pose.orientation.y;//0.0;
marker.pose.orientation.z = q_z;//rosbag_marker->pose.orientation.z;//0.0;
marker.pose.orientation.w = q_w;//rosbag_marker->pose.orientation.w; //1.0;
// Set the scale of the marker -- 1x1x1 here means 1m on a side
marker.scale.x = 0.1;//rosbag_marker->scale.x; //1.0;
marker.scale.y = 0.165;//rosbag_marker->scale.y; //1.0;
marker.scale.z = 0.1;//rosbag_marker->scale.z; //0.5;
// Set the color -- be sure to set alpha to something non-zero!
marker.color.r = 1.0 ; // * ( (double)(total_markers - index) / (double)total_markers);//rosbag_marker->color.r; //0.0f;
marker.color.g = 0.0f; // * ( (double)index / (double)total_markers); //1.0f;//rosbag_marker->color.g; //1.0f;
marker.color.b = 1.0f;//rosbag_marker->color.b; //0.0f;
marker.color.a = 0.75f;//1.0 * ( (double)(total_markers - index) / (double)total_markers); //rosbag_marker->color.a; //1.0;
marker.lifetime = ros::Duration();
// Publish the marker
while (marker_pub.getNumSubscribers() < 1)
{
if (!ros::ok())
{
ROS_WARN_ONCE("NOT OK!");
break;
}
ROS_WARN_ONCE("Please create a subscriber to the marker");
sleep(1.0);
}
marker_pub.publish(marker);
// std::cout << "success?" << std::endl;
}
/*!
* \brief Publishes the current joint angles.
*
* Joint angles are published in both their raw state as obtained from the arm
* (JointAngles), and transformed & converted to radians (joint_state) as per
* the Kinova Kinematics PDF.
*
* Velocities and torques (effort) are only published in the JointStates
* message, only for the first 6 joints as these values are not available for
* the fingers.
*/
void KinovaArm::publishJointAngles(void)
{
FingerAngles fingers;
kinova_comm_.getFingerPositions(fingers);
if (arm_joint_number_ != 4 && arm_joint_number_ != 6)
{
ROS_WARN_ONCE("The joint_state publisher only supports 4DOF and 6DOF for now.: %d", arm_joint_number_);
}
// Query arm for current joint angles
KinovaAngles current_angles;
kinova_comm_.getJointAngles(current_angles);
kinova_msgs::JointAngles kinova_angles = current_angles.constructAnglesMsg();
AngularPosition joint_command;
kinova_comm_.getAngularCommand(joint_command);
kinova_msgs::JointAngles joint_command_msg = KinovaAngles(joint_command.Actuators).constructAnglesMsg();
sensor_msgs::JointState joint_state;
joint_state.name = joint_names_;
joint_state.header.stamp = ros::Time::now();
// Transform from Kinova DH algorithm to physical angles in radians, then place into vector array
joint_state.position.resize(joint_total_number_);
joint_state.position[0] = kinova_angles.joint1 * M_PI/180;
joint_state.position[1] = kinova_angles.joint2 * M_PI/180;
joint_state.position[2] = kinova_angles.joint3 * M_PI/180;
joint_state.position[3] = kinova_angles.joint4 * M_PI/180;
if (arm_joint_number_ == 6)
{
joint_state.position[4] = kinova_angles.joint5 * M_PI/180;
joint_state.position[5] = kinova_angles.joint6 * M_PI/180;
}
if(finger_number_==2)
{
joint_state.position[joint_total_number_-2] = 0;
joint_state.position[joint_total_number_-1] = 0;
}
else if(finger_number_==3)
{
joint_state.position[joint_total_number_-3] = 0;
joint_state.position[joint_total_number_-2] = 0;
joint_state.position[joint_total_number_-1] = 0;
}
// Joint velocities
KinovaAngles current_vels;
kinova_comm_.getJointVelocities(current_vels);
joint_state.velocity.resize(joint_total_number_);
joint_state.velocity[0] = current_vels.Actuator1;
joint_state.velocity[1] = current_vels.Actuator2;
joint_state.velocity[2] = current_vels.Actuator3;
joint_state.velocity[3] = current_vels.Actuator4;
// no velocity info for fingers
if(finger_number_==2)
{
joint_state.velocity[joint_total_number_-2] = 0;
joint_state.velocity[joint_total_number_-1] = 0;
}
else if(finger_number_==3)
{
joint_state.velocity[joint_total_number_-3] = 0;
joint_state.velocity[joint_total_number_-2] = 0;
joint_state.velocity[joint_total_number_-1] = 0;
}
if (arm_joint_number_ == 6)
{
joint_state.velocity[4] = current_vels.Actuator5;
joint_state.velocity[5] = current_vels.Actuator6;
}
// ROS_DEBUG_THROTTLE(0.1,
// "Raw joint velocities: %f %f %f %f %f %f",
// current_vels.Actuator1,
// current_vels.Actuator2,
// current_vels.Actuator3,
// current_vels.Actuator4,
// current_vels.Actuator5,
// current_vels.Actuator6);
if (convert_joint_velocities_) {
convertKinDeg(joint_state.velocity);
}
// Joint torques (effort)
// NOTE: Currently invalid.
KinovaAngles joint_tqs;
joint_state.effort.resize(joint_total_number_);
joint_state.effort[0] = joint_tqs.Actuator1;
joint_state.effort[1] = joint_tqs.Actuator2;
joint_state.effort[2] = joint_tqs.Actuator3;
joint_state.effort[3] = joint_tqs.Actuator4;
// no effort info for fingers
if(finger_number_==2)
{
joint_state.effort[joint_total_number_-2] = 0;
joint_state.effort[joint_total_number_-1] = 0;
}
else if(finger_number_==3)
{
joint_state.effort[joint_total_number_-3] = 0;
joint_state.effort[joint_total_number_-2] = 0;
joint_state.effort[joint_total_number_-1] = 0;
}
if (arm_joint_number_ == 6)
{
joint_state.effort[4] = joint_tqs.Actuator5;
joint_state.effort[5] = joint_tqs.Actuator6;
}
joint_angles_publisher_.publish(kinova_angles);
joint_command_publisher_.publish(joint_command_msg);
joint_state_publisher_.publish(joint_state);
}
// Publish recorded markers but also push waypoint trajectories.
void pub_recorded_marker(ros::Publisher &marker_pub, visualization_msgs::Marker::ConstPtr &rosbag_marker,
int index, int total_markers, tf::Transform &translate_to_main, tf::Transform &rotate_to_main ){
tf::Vector3 marker_position (rosbag_marker->pose.position.x,
rosbag_marker->pose.position.y,
rosbag_marker->pose.position.z);
tf::Quaternion marker_orientation (rosbag_marker->pose.orientation.x,
rosbag_marker->pose.orientation.y,
rosbag_marker->pose.orientation.z,
rosbag_marker->pose.orientation.w);
// Perform tf transformation.
marker_position = translate_to_main * marker_position;
marker_orientation = rotate_to_main * marker_orientation;
visualization_msgs::Marker marker;
uint32_t shape = visualization_msgs::Marker::CUBE;
// Set the frame ID and timestamp. See the TF tutorials for information on these.
//marker.header.frame_id = "/my_frame";
marker.header.frame_id = "/base_link";
marker.header.stamp = ros::Time::now();
// Set the namespace and id for this marker. This serves to create a unique ID
// Any marker sent with the same namespace and id will overwrite the old one
marker.ns = rosbag_marker->ns;//"basic_shapes";
marker.id = index;//rosbag_marker->id;
// Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
marker.type = rosbag_marker->type; //shape;
// Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
marker.action = visualization_msgs::Marker::ADD;
// Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header
marker.pose.position.x = marker_position.getX();//rosbag_marker->pose.position.x;//0;
marker.pose.position.y = marker_position.getY();//rosbag_marker->pose.position.y;//0;
marker.pose.position.z = marker_position.getZ();//rosbag_marker->pose.position.z;//0;
marker.pose.orientation.x = marker_orientation.getAxis().getX();//rosbag_marker->pose.orientation.x;//0.0;
marker.pose.orientation.y = marker_orientation.getAxis().getY();//rosbag_marker->pose.orientation.y;//0.0;
marker.pose.orientation.z = marker_orientation.getAxis().getZ();//rosbag_marker->pose.orientation.z;//0.0;
marker.pose.orientation.w = marker_orientation.getW();//rosbag_marker->pose.orientation.w; //1.0;
// Set the scale of the marker -- 1x1x1 here means 1m on a side
marker.scale.x = rosbag_marker->scale.x; //1.0;
marker.scale.y = rosbag_marker->scale.y; //1.0;
marker.scale.z = rosbag_marker->scale.z; //0.5;
// Set the color -- be sure to set alpha to something non-zero!
// use index/total_markers percentage as a way to fade from red to green.
marker.color.r = 1.0 * ( (double)(total_markers - index) / (double)total_markers);//rosbag_marker->color.r; //0.0f;
marker.color.g = 1.0f * ( (double)index / (double)total_markers); //1.0f;//rosbag_marker->color.g; //1.0f;
marker.color.b = 0.0f;//rosbag_marker->color.b; //0.0f;
marker.color.a = 1.0f;//1.0 * ( (double)(total_markers - index) / (double)total_markers); //rosbag_marker->color.a; //1.0;
// Make it last forever. We don't need to keep publishing it.
marker.lifetime = ros::Duration();
// Push marker 6DoF Pose to the waypoints vector.
geometry_msgs::Pose target_pose;
target_pose.position.x = marker_position.getX();
target_pose.position.y = marker_position.getY();
target_pose.position.z = marker_position.getZ();
target_pose.orientation.x = marker_orientation.getAxis().getX();//rosbag_marker->pose.orientation.x;//0.0;
target_pose.orientation.y = marker_orientation.getAxis().getY();//rosbag_marker->pose.orientation.y;//0.0;
target_pose.orientation.z = marker_orientation.getAxis().getZ();//rosbag_marker->pose.orientation.z;//0.0;
target_pose.orientation.w = marker_orientation.getW();//rosbag_marker->pose.orientation.w; //1.0;
waypoints.push_back(target_pose);
// Publish the marker
while (marker_pub.getNumSubscribers() < 1)
{
if (!ros::ok())
{
ROS_WARN_ONCE("NOT OK!");
break;
}
ROS_WARN_ONCE("Please create a subscriber to the marker");
sleep(1.0);
}
marker_pub.publish(marker);
}
bool Depth::processCameraInfo(
const sensor_msgs::CameraInfoConstPtr& first_camera_info,
const sensor_msgs::CameraInfoConstPtr& second_camera_info, double* baseline,
double* focal_length, bool* first_is_left, int* cx, int* cy) {
if (first_camera_info->height != second_camera_info->height) {
ROS_ERROR("Image heights do not match");
return false;
}
if (first_camera_info->width != second_camera_info->width) {
ROS_ERROR("Image widths do not match");
return false;
}
for (double d : first_camera_info->D) {
if (!ApproxEq(d, 0)) {
ROS_ERROR("First image has non-zero distortion");
return false;
}
}
for (double d : second_camera_info->D) {
if (!ApproxEq(d, 0)) {
ROS_ERROR("Second image has non-zero distortion");
return false;
}
}
for (size_t i = 0; i < 12; ++i) {
if ((i != 3) &&
!ApproxEq(first_camera_info->P[i], second_camera_info->P[i])) {
ROS_ERROR("Image P matrices must match (excluding x offset)");
return false;
}
}
if (!ApproxEq(first_camera_info->P[1], 0) ||
!ApproxEq(first_camera_info->P[4], 0)) {
ROS_ERROR("Image P matrix contains skew");
return false;
}
if (!ApproxEq(first_camera_info->P[0], first_camera_info->P[5])) {
ROS_ERROR("Image P matrix has different values for Fx and Fy");
return false;
}
if (first_camera_info->P[0] <= 0) {
ROS_ERROR("Focal length must be greater than 0");
return false;
}
// downgraded to warning so that the color images of the KITTI dataset can be
// processed
if (!ApproxEq(first_camera_info->P[8], 0) ||
!ApproxEq(first_camera_info->P[9], 0) ||
!ApproxEq(first_camera_info->P[10], 1) ||
!ApproxEq(first_camera_info->P[11], 0)) {
ROS_WARN_ONCE(
"Image P matrix does not end in [0,0,1,0], these values will be "
"ignored");
}
// again downgraded to warning because KITTI has ugly matrices
if (!ApproxEq(first_camera_info->P[7], 0)) {
ROS_WARN_ONCE("P contains Y offset, this value will be ignored");
}
*focal_length = first_camera_info->P[0];
*baseline = (second_camera_info->P[3] - first_camera_info->P[3]) /
first_camera_info->P[0];
if (*baseline > 0) {
*first_is_left = false;
} else {
*first_is_left = true;
*baseline *= -1;
}
*cx = first_camera_info->P[2];
*cy = first_camera_info->P[6];
return true;
}