/**
* 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
