/** * Receives the messages from the IMU */ void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn) { // get RPY double roll, pitch, yaw; tf::Quaternion orientation; tf::quaternionMsgToTF(imuIn->orientation, orientation); tf::Quaternion transformation_q; // -90 in y transformation_q.setRotation( tf::Vector3(0,1,0), -tfScalar(PI/2)); tf::Quaternion parsed_orientation = orientation.operator*=(transformation_q); geometry_msgs::PoseStamped imuVis; // publish the imu quaternion imuVis.pose.orientation.x = parsed_orientation.x(); imuVis.pose.orientation.y = parsed_orientation.y(); imuVis.pose.orientation.z = parsed_orientation.z(); imuVis.pose.orientation.w = parsed_orientation.w(); imuVis.pose.position.x = 0; imuVis.pose.position.y = 0; imuVis.pose.position.z = 0; imuVis.header.stamp = imuIn->header.stamp; imuVis.header.frame_id = "/camera_init_2"; pubImuVisPointer->publish(imuVis); tf::Matrix3x3(parsed_orientation).getRPY(roll, pitch, yaw); int imuPointerBack = imuPointerLast; // initially -1 imuPointerLast = (imuPointerLast + 1) % imuQueLength; // the array pointer 0 - imuQueueLength (100 in MS case) imuTime[imuPointerLast] = imuIn->header.stamp.toSec(); // the time the imu measurement was taken for each of the imuQueueLength (100) points double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack]; // difference in time between the two imu measurement // if two consecutive measurement were taken within 0.1 seconds if (timeDiff < 0.1) { // imuAccuRoll += timeDiff * imuIn->angular_velocity.x; // imuAccuPitch += timeDiff * imuIn->angular_velocity.y; // imuAccuYaw += timeDiff * imuIn->angular_velocity.z; imuRoll[imuPointerLast] = yaw; imuPitch[imuPointerLast] = roll; imuYaw[imuPointerLast] = -pitch; // imuRoll[imuPointerLast] = imuAccuRoll; // imuPitch[imuPointerLast] = -imuAccuPitch; // imuYaw[imuPointerLast] = -imuAccuYaw; // if there's monotonic IMU shift- accumulate it // imuAccX[imuPointerLast] = -imuIn->linear_acceleration.y; // imuAccY[imuPointerLast] = -imuIn->linear_acceleration.z - 9.81; // imuAccZ[imuPointerLast] = imuIn->linear_acceleration.x; AccumulateIMUShift(); } }
int main(int argc, char* argv[]) { ros::init(argc,argv,"tilt_controller"); ros::NodeHandle nh; ros::Publisher angle_pub = nh.advertise<std_msgs::UInt16>("/servo",1000,true); ros::Publisher sweep_pub = nh.advertise<std_msgs::String>("/sweep",1000); static tf::TransformBroadcaster br; ros::Rate loop_rate(30); int angle = 11; int increment = 1; std_msgs::String string_msg; string_msg.data = "sweep"; ROS_INFO("%s","Starting tilt_controller node..."); while(ros::ok()) { std_msgs::UInt16 angle_msg; angle_msg.data = angle; tf::Transform transform; transform.setRotation(tf::Quaternion(tf::Vector3(-1,0,0),tfScalar(degreesToRadians(angle)))); angle_pub.publish(angle_msg); br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"base_link","laser")); ros::spinOnce(); if(angle == 170) { increment = -1; sweep_pub.publish(string_msg); } if(angle == 10) { increment = 1; sweep_pub.publish(string_msg); } angle += increment; loop_rate.sleep(); } return 0; }
tf::Quaternion leatherman::setRPY(const tfScalar& roll, const tfScalar& pitch, const tfScalar& yaw) { tfScalar halfYaw = tfScalar(yaw) * tfScalar(0.5); tfScalar halfPitch = tfScalar(pitch) * tfScalar(0.5); tfScalar halfRoll = tfScalar(roll) * tfScalar(0.5); tfScalar cosYaw = tfCos(halfYaw); tfScalar sinYaw = tfSin(halfYaw); tfScalar cosPitch = tfCos(halfPitch); tfScalar sinPitch = tfSin(halfPitch); tfScalar cosRoll = tfCos(halfRoll); tfScalar sinRoll = tfSin(halfRoll); tf::Quaternion q; q.setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx return q; }
/** @brief Markers publication for the visualization of the calibration ressult on rviz @param[in] clouds ball center acquisitions in all sensors @param[in] lasers position of the sensors @return vector<visualization_msgs::Marker> */ vector<visualization_msgs::Marker> createTargetMarkers(vector<pcl::PointCloud<pcl::PointXYZ> > clouds, vector<geometry_msgs::Pose> lasers, vector<string> deviceNames, const vector<double>& RPY, const vector<double>& translation ) { tf::Transform t_rpy; tf::Quaternion q; if (translation.empty() && RPY.empty()) //user does not want to translate/rotate clouds and sensors. { t_rpy.setOrigin( tf::Vector3 (tfScalar(0), tfScalar(0), tfScalar(0)) ); // no translation is done q = tf::createQuaternionFromRPY(0.0, 0.0, 0.0 ); // no rotation t_rpy.setRotation( q ); } else if (translation.empty()) // only rotation given by the user, no translation { t_rpy.setOrigin( tf::Vector3 (tfScalar(0), tfScalar(0), tfScalar(0)) ); // no translation q = tf::createQuaternionFromRPY( RPY[0], RPY[1], RPY[2] ); // quaternion computation from given angles t_rpy.setRotation( q ); } else // rotation and translation given by the user { t_rpy.setOrigin( tf::Vector3 (tfScalar(translation[0]), tfScalar(translation[1]), tfScalar(translation[2])) ); // translation given by the user q = tf::createQuaternionFromRPY( RPY[0], RPY[1], RPY[2] ); // quaternion computation from given angles t_rpy.setRotation( q ); } static Markers marker_list; int nLasers=lasers.size(); std::cout << "number of lasers: " << nLasers << std::endl; //Reduce the elements status, ADD to REMOVE and REMOVE to delete marker_list.decrement(); // Create a colormap class_colormap colormap("hsv", nLasers, 1, false); visualization_msgs::Marker marker_centers; visualization_msgs::Marker marker_lasers[nLasers*5]; // *5 to make space for Y, Z axis and square that represents the laser marker_centers.header.frame_id = "/my_frame3"; marker_centers.header.stamp = ros::Time::now(); marker_centers.ns = "centers"; marker_centers.id = 0; marker_centers.action = visualization_msgs::Marker::ADD; marker_centers.type = visualization_msgs::Marker::SPHERE_LIST; marker_centers.scale.x = 0.08; marker_centers.scale.y = 0.08; marker_centers.scale.z = 0.08; marker_centers.pose.orientation.x=q[0]; marker_centers.pose.orientation.y=q[1]; marker_centers.pose.orientation.z=q[2]; marker_centers.pose.orientation.w=q[3]; marker_lasers[0].color.a=1; marker_lasers[0].color.g=1; marker_lasers[1].color.b=1; marker_lasers[1].color.a=1; marker_lasers[2].color.r=1; marker_lasers[2].color.a=1; for(int n=0; n<clouds.size(); n++) { pcl::PointCloud<pcl::PointXYZ> cloud = clouds[n]; std_msgs::ColorRGBA color; color = colormap.color(n); for ( int i = 0; i< cloud.points.size(); i++) { geometry_msgs::Point pt; pt.x= cloud.points[i].x; pt.y= cloud.points[i].y; pt.z= cloud.points[i].z; marker_centers.points.push_back(pt); marker_centers.colors.push_back(color); } //end for marker_list.update(marker_centers); } //position and orientation of laser int counter = 0; for(int n=0; n<lasers.size(); n++) { std::cout << "laser "<< n << std::endl; geometry_msgs::Pose laser = lasers[n]; tf::Transform t_laser; t_laser.setOrigin( tf::Vector3(laser.position.x, laser.position.y, laser.position.z) ); tf::Quaternion q_laser(laser.orientation.x, laser.orientation.y, laser.orientation.z, laser.orientation.w); t_laser.setRotation(q_laser); std::cout << "laser transform" << std::endl; for (int k=0; k<5; k++) { marker_lasers[counter].header.frame_id = "/my_frame3"; marker_lasers[counter].header.stamp = ros::Time::now(); std::stringstream ss; ss << "Laser " << n << "Marker: " << k; marker_lasers[counter].ns = ss.str(); marker_lasers[counter].action = visualization_msgs::Marker::ADD; std_msgs::ColorRGBA color; if (k<3) { marker_lasers[counter].type = visualization_msgs::Marker::ARROW; marker_lasers[counter].scale.x = 0.5; marker_lasers[counter].scale.y = 0.05; marker_lasers[counter].scale.z = 0.05; color.r = 0; color.g = 0; color.b = 0; color.a = 1; } else if (k == 3) { marker_lasers[counter].type = visualization_msgs::Marker::CUBE; marker_lasers[counter].scale.x = 0.15; marker_lasers[counter].scale.y = 0.15; marker_lasers[counter].scale.z = 0.15; } else { marker_lasers[counter].type = visualization_msgs::Marker::TEXT_VIEW_FACING; marker_lasers[counter].text = deviceNames[n]; marker_lasers[counter].scale.x = 0.15; marker_lasers[counter].scale.y = 0.15; marker_lasers[counter].scale.z = 0.15; std::cout << deviceNames[n] << std::endl; } tf::Transform t_aux; tf::Quaternion q_aux; t_aux.setOrigin( tf::Vector3 (tfScalar(0), tfScalar(0), tfScalar(0)) ); // no translation is done switch (k) { case 0: { q_aux = tf::createQuaternionFromRPY(0.0, 0.0, 0.0 ); // no rotation->this is the X axis color.r = 1.0; //std::cout << "2 " << k << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << color << std::endl; break; } case 1: { q_aux = tf::createQuaternionFromRPY( 0.0, 0.0, M_PI/2 ); // rotation to get Y axis from X color.g = 1.0; //std::cout << "2 " << k << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << color << std::endl; break; } case 2: { q_aux = tf::createQuaternionFromRPY( 0.0, -M_PI/2, 0.0); // rotation to get Z axis from X color.b = 1.0; //std::cout << "2 " << k << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << color << std::endl; break; } case 3: { q_aux = tf::createQuaternionFromRPY( 0.0, 0.0, 0.0); // no rotation->this is the square at the axes origin that represents the sensor color = colormap.color(n); //std::cout << "2 " << k << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << color << std::endl; break; } default: { std::cout << "text" << std::endl; q_aux = tf::createQuaternionFromRPY( 0.0, 0.0, 0.0); // no rotation->this is the square at the axes origin that represents the sensor color.r = 1.0; color.g = 1.0; color.b = 1.0; color.a = 1.0; } } t_aux.setRotation( q_aux ); tf::Transform transform_laser = t_rpy * t_laser * t_aux; tf::Quaternion rotation_laser = transform_laser.getRotation(); tf::Vector3 translation_laser = transform_laser.getOrigin(); std::cout << "Transformation" << std::endl; std::cout << "translation " << translation_laser[0] << " " << translation_laser[1] << " " << translation_laser[2] << std::endl; std::cout << "rotation " << rotation_laser[0] << " " << rotation_laser[1] << " " << rotation_laser[2] << " " << rotation_laser[3] << std::endl; marker_lasers[counter].pose.position.y=translation_laser[1]; if (k!=4) { marker_lasers[counter].pose.position.x=translation_laser[0]; marker_lasers[counter].pose.position.z=translation_laser[2]; std::cout << "k!4" << std::endl; } else { marker_lasers[counter].pose.position.x=translation_laser[0]+0.15; marker_lasers[counter].pose.position.z=translation_laser[2]+0.25; std::cout << "text" << std::endl; } marker_lasers[counter].pose.orientation.x=rotation_laser[0]; marker_lasers[counter].pose.orientation.y=rotation_laser[1]; marker_lasers[counter].pose.orientation.z=rotation_laser[2]; marker_lasers[counter].pose.orientation.w=rotation_laser[3]; std::cout << "pose" << std::endl; marker_lasers[counter].color=color; std::cout << "color" << std::endl; marker_list.update(marker_lasers[counter]); std::cout << "update" << std::endl; counter++; } } std::cout << "clean" << std::endl; //Remove markers that should not be transmitted marker_list.clean(); std::cout << "finished" << std::endl; //Clean the marker_vector and put new markers in it; return marker_list.getOutgoingMarkers(); } //end function
tfScalar Deg2Rad(float deg) { return tfScalar(deg*3.1415926/180.0); }
/** @brief Markers publication for the visualization of the calibration ressult on rviz @param[in] clouds ball center acquisitions in all sensors @param[in] lasers position of the sensors @return vector<visualization_msgs::Marker> */ vector<visualization_msgs::Marker> createTargetMarkers(vector<pcl::PointCloud<pcl::PointXYZ> > clouds, vector<geometry_msgs::Pose> lasers, const vector<double>& RPY, const vector<double>& translation ) { tf::Transform t_rpy; tf::Quaternion q; if (translation.empty() && RPY.empty()) //user does not want to translate/rotate clouds and sensors. { t_rpy.setOrigin( tf::Vector3 (tfScalar(0), tfScalar(0), tfScalar(0)) ); // no translation is done q = tf::createQuaternionFromRPY(0.0, 0.0, 0.0 ); // no rotation t_rpy.setRotation( q ); } else if (translation.empty()) // only rotation given by the user, no translation { t_rpy.setOrigin( tf::Vector3 (tfScalar(0), tfScalar(0), tfScalar(0)) ); // no translation q = tf::createQuaternionFromRPY( RPY[0], RPY[1], RPY[2] ); // quaternion computation from given angles t_rpy.setRotation( q ); } else // rotation and translation given by the user { t_rpy.setOrigin( tf::Vector3 (tfScalar(translation[0]), tfScalar(translation[1]), tfScalar(translation[2])) ); // translation given by the user q = tf::createQuaternionFromRPY( RPY[0], RPY[1], RPY[2] ); // quaternion computation from given angles t_rpy.setRotation( q ); } static Markers marker_list; int nLasers=lasers.size(); //Reduce the elements status, ADD to REMOVE and REMOVE to delete marker_list.decrement(); // Create a colormap class_colormap colormap("hsv",10, 1, false); visualization_msgs::Marker marker_centers; visualization_msgs::Marker marker_lasers[nLasers]; marker_centers.header.frame_id = "/my_frame3"; marker_centers.header.stamp = ros::Time::now(); marker_centers.ns = "centers"; marker_centers.action = visualization_msgs::Marker::ADD; marker_centers.type = visualization_msgs::Marker::SPHERE_LIST; marker_centers.scale.x = 0.08; marker_centers.scale.y = 0.08; marker_centers.scale.z = 0.08; marker_centers.pose.orientation.x=q[0]; marker_centers.pose.orientation.y=q[1]; marker_centers.pose.orientation.z=q[2]; marker_centers.pose.orientation.w=q[3]; /*marker_centers.color.b=1; marker_centers.color.a=1;*/ marker_lasers[0].color.a=1; marker_lasers[0].color.g=1; marker_lasers[1].color.b=1; marker_lasers[1].color.a=1; marker_lasers[2].color.r=1; marker_lasers[2].color.a=1; for(int n=0; n<clouds.size(); n++) { { pcl::PointCloud<pcl::PointXYZ> cloud = clouds[n]; std_msgs::ColorRGBA color; color = colormap.color(n); for ( int i = 0; i< cloud.points.size(); i++) { geometry_msgs::Point pt; pt.x= cloud.points[i].x; pt.y= cloud.points[i].y; pt.z= cloud.points[i].z; marker_centers.points.push_back(pt); marker_centers.colors.push_back(color); } //end for marker_list.update(marker_centers); } } //position and orientation of laser for(int n=0; n<lasers.size(); n++) { { marker_lasers[n].header.frame_id = "/my_frame3"; marker_lasers[n].header.stamp = ros::Time::now(); std::stringstream ss; ss << "Laser " << n; marker_lasers[n].ns = ss.str(); marker_lasers[n].action = visualization_msgs::Marker::ADD; marker_lasers[n].type = visualization_msgs::Marker::ARROW; marker_lasers[n].scale.x = 0.2; marker_lasers[n].scale.y = 0.1; marker_lasers[n].scale.z = 0.1; geometry_msgs::Pose laser = lasers[n]; std_msgs::ColorRGBA color; color = colormap.color(n); tf::Transform t_laser; t_laser.setOrigin( tf::Vector3(laser.position.x, laser.position.y, laser.position.z) ); tf::Quaternion q_laser(laser.orientation.x, laser.orientation.y, laser.orientation.z, laser.orientation.w); t_laser.setRotation(q_laser); t_laser= t_rpy * t_laser; q_laser = t_laser.getRotation(); tf::Vector3 trans_laser = t_laser.getOrigin(); marker_lasers[n].pose.position.x=trans_laser[0]; marker_lasers[n].pose.position.y=trans_laser[1]; marker_lasers[n].pose.position.z=trans_laser[2]; marker_lasers[n].pose.orientation.x=q_laser[0]; marker_lasers[n].pose.orientation.y=q_laser[1]; marker_lasers[n].pose.orientation.z=q_laser[2]; marker_lasers[n].pose.orientation.w=q_laser[3]; marker_lasers[n].color=color; marker_list.update(marker_lasers[n]); } } //Remove markers that should not be transmitted marker_list.clean(); //Clean the marker_vector and put new markers in it; return marker_list.getOutgoingMarkers(); } //end function