geometry_msgs::Quaternion conversions::KDLRotToQuaternionMsg(const KDL::Rotation & in){
geometry_msgs::Quaternion out;
in.GetQuaternion(out.x, out.y, out.z, out.w);
return out;
}
void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
{
PointCloud cloud;
pcl::fromROSMsg(*msg, cloud);
g_cloud_frame = cloud.header.frame_id;
g_cloud_ready = true;
if(!g_head_depth_ready) return;
Mat img3D;
img3D = Mat::zeros(cloud.height, cloud.width, CV_32FC3);
//img3D.create(cloud.height, cloud.width, CV_32FC3);
int yMin, xMin, yMax, xMax;
yMin = 0; xMin = 0;
yMax = img3D.rows; xMax = img3D.cols;
//get 3D from depth
for(int y = yMin ; y < img3D.rows; y++) {
Vec3f* img3Di = img3D.ptr<Vec3f>(y);
for(int x = xMin; x < img3D.cols; x++) {
pcl::PointXYZ p = cloud.at(x,y);
if((p.z>g_head_depth-0.2) && (p.z<g_head_depth+0.2)) {
img3Di[x][0] = p.x*1000;
img3Di[x][1] = p.y*1000;
img3Di[x][2] = hypot(img3Di[x][0], p.z*1000); //they seem to have trained with incorrectly projected 3D points
//img3Di[x][2] = p.z*1000;
} else {
img3Di[x] = 0;
}
}
}
g_means.clear();
g_votes.clear();
g_clusters.clear();
//do the actual estimate
estimator.estimate( img3D,
g_means,
g_clusters,
g_votes,
g_stride,
g_maxv,
g_prob_th,
g_larger_radius_ratio,
g_smaller_radius_ratio,
false,
g_th
);
//assuming there's only one head in the image!
if(g_means.size() > 0) {
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = msg->header.frame_id;
cv::Vec<float,POSE_SIZE> pose(g_means[0]);
KDL::Rotation r = KDL::Rotation::RPY(
from_degrees( pose[5]+180+g_roll_bias ),
from_degrees(-pose[3]+180+g_pitch_bias),
from_degrees(-pose[4]+ g_yaw_bias )
);
double qx, qy, qz, qw;
r.GetQuaternion(qx, qy, qz, qw);
geometry_msgs::PointStamped head_point;
geometry_msgs::PointStamped head_point_transformed;
head_point.header = pose_msg.header;
head_point.point.x = pose[0]/1000;
head_point.point.y = pose[1]/1000;
head_point.point.z = pose[2]/1000;
try {
listener->waitForTransform(head_point.header.frame_id, g_head_target_frame, ros::Time::now(), ros::Duration(2.0));
listener->transformPoint(g_head_target_frame, head_point, head_point_transformed);
} catch(tf::TransformException ex) {
ROS_WARN(
"Transform exception, when transforming point from %s to %s\ndropping pose (don't worry, this is probably ok)",
head_point.header.frame_id.c_str(), g_head_target_frame.c_str());
ROS_WARN("Exception was %s", ex.what());
return;
}
tf::Transform trans;
pose_msg.pose.position = head_point_transformed.point;
pose_msg.header.frame_id = head_point_transformed.header.frame_id;
pose_msg.pose.orientation.x = qx;
pose_msg.pose.orientation.y = qy;
pose_msg.pose.orientation.z = qz;
pose_msg.pose.orientation.w = qw;
trans.setOrigin(tf::Vector3(
pose_msg.pose.position.x,
pose_msg.pose.position.y,
pose_msg.pose.position.z
));
trans.setRotation(tf::Quaternion(qx, qy, qz, qw));
g_transform = tf::StampedTransform(trans, pose_msg.header.stamp, pose_msg.header.frame_id, "head_origin");
// broadcaster->sendTransform(tf::StampedTransform(trans, pose_msg.header.stamp, pose_msg.header.frame_id, "head_origin"));
g_transform_ready = true;
pose_msg.header.stamp = ros::Time::now();
geometry_msgs::PoseStamped zero_pose;
zero_pose.header.frame_id = "head_origin";
zero_pose.header.stamp = ros::Time::now();
zero_pose.pose.orientation.w = 1;
//pose_pub.publish(pose_msg);
pose_pub.publish(zero_pose);
}
}
bool CartesianGenerator::generateNewVelocityProfiles(RTT::base::PortInterface* portInterface){
m_time_passed = os::TimeService::Instance()->secondsSince(m_time_begin);
geometry_msgs::Pose pose;
cmdCartPose.read(pose);
#if 1
std::cout << "A new pose arrived" << std::endl;
std::cout << "position: " << pose.position.x << " " << pose.position.y << " " << pose.position.z << std::endl;
#endif
m_traject_end.p.x(pose.position.x);
m_traject_end.p.y(pose.position.y);
m_traject_end.p.z(pose.position.z);
m_traject_end.M=Rotation::Quaternion(pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w);
// get current position
geometry_msgs::Pose pose_meas;
m_position_meas_port.read(pose_meas);
m_traject_begin.p.x(pose_meas.position.x);
m_traject_begin.p.y(pose_meas.position.y);
m_traject_begin.p.z(pose_meas.position.z);
m_traject_begin.M=Rotation::Quaternion(pose_meas.orientation.x,pose_meas.orientation.y,pose_meas.orientation.z,pose_meas.orientation.w);
KDL::Rotation errorRotation = (m_traject_end.M)*(m_traject_begin.M.Inverse());
// KDL::Rotation tmp=errorRotation;
// double q1, q2, q3, q4;
// cout << "tmp" << endl;
// cout << tmp(0,0) << ", " << tmp(0,1) << ", " << tmp(0,2) << endl;
// cout << tmp(1,0) << ", " << tmp(1,1) << ", " << tmp(1,2) << endl;
// cout << tmp(2,0) << ", " << tmp(2,1) << ", " << tmp(2,2) << endl;
// cout << endl;
// tmp.GetQuaternion(q1,q2,q3,q4);
// cout << "tmp quaternion: " << q1 << q2 << q3 << q4 << endl;
double x,y,z,w;
errorRotation.GetQuaternion(x,y,z,w);
Eigen::AngleAxis<double> aa;
aa = Eigen::Quaterniond(w,x,y,z);
currentRotationalAxis = aa.axis();
deltaTheta = aa.angle();
// std::cout << "----EIGEN---------" << std::endl << "currentRotationalAxis: " << std::endl << currentRotationalAxis << std::endl;
// std::cout << "deltaTheta" << deltaTheta << std::endl;
// currentRotationalAxis[0]=x;
// currentRotationalAxis[1]=y;
// currentRotationalAxis[2]=z;
// if(currentRotationalAxis.norm() > 0.001){
// currentRotationalAxis.normalize();
// deltaTheta = 2*acos(w);
// }else{
// currentRotationalAxis.setZero();
// deltaTheta = 0.0;
// }
// std::cout << "----KDL-----------" << std::endl << "currentRotationalAxis: " << std::endl << currentRotationalAxis << std::endl;
// std::cout << "deltaTheta" << deltaTheta << std::endl;
std::vector<double> cartPositionCmd = std::vector<double>(3,0.0);
cartPositionCmd[0] = pose.position.x;
cartPositionCmd[1] = pose.position.y;
cartPositionCmd[2] = pose.position.z;
std::vector<double> cartPositionMsr = std::vector<double>(3,0.0);
//the following 3 assignments will get over written except for the first time
cartPositionMsr[0] = pose_meas.position.x;
cartPositionMsr[1] = pose_meas.position.y;
cartPositionMsr[2] = pose_meas.position.z;
std::vector<double> cartVelocity = std::vector<double>(3,0.0);
if ((int)motionProfile.size() == 0){//Only for the first run
for(int i = 0; i < 3; i++)
{
cartVelocity[i] = 0.0;
}
}else{
for(int i = 0; i < 3; i++)
{
cartVelocity[i] = motionProfile[i].Vel(m_time_passed);
cartPositionMsr[i] = motionProfile[i].Pos(m_time_passed);
}
}
motionProfile.clear();
// Set motion profiles
for(int i = 0; i < 3; i++){
motionProfile.push_back(VelocityProfile_NonZeroInit(m_maximum_velocity[i], m_maximum_acceleration[i]));
motionProfile[i].SetProfile(cartPositionMsr[i], cartPositionCmd[i], cartVelocity[i]);
}
//motionProfile for theta
motionProfile.push_back(VelocityProfile_NonZeroInit(m_maximum_velocity[3], m_maximum_acceleration[3]));
motionProfile[3].SetProfile(0.0,deltaTheta,0.0);
m_time_begin = os::TimeService::Instance()->getTicks();
m_time_passed = 0;
return true;
}
// construct rotation
urdf::Rotation toUrdf(const KDL::Rotation & r)
{
double x,y,z,w;
r.GetQuaternion(x,y,z,w);
return urdf::Rotation(x,y,z,w);
}
int main(int argc, char **argv)
{
float x=0.0f;
float y=0.0f;
float z=0.0f;
float roll=0.0f;
float pitch=0.0f;
float yaw=0.0f;
ros::init(argc, argv, "dan_tester");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe( "lar_comau/comau_joint_states",1,state_callback );
ros::Publisher joint_state_pub = n.advertise<sensor_msgs::JointState>("lar_comau/comau_joint_state_publisher", 1);
ros::Publisher cartesian_controller = n.advertise<lar_comau::ComauCommand>("lar_comau/comau_cartesian_controller", 1);
ros::Rate loop_rate(10);
//robot
std::string robot_desc_string;
n.param("robot_description", robot_desc_string, std::string());
lar_comau::ComauSmartSix robot(robot_desc_string,"base_link", "link6");
//ROS_INFO("\n\n%s\n\n",argv[1]);
initPoses();
int count = 0;
while (ros::ok())
{
sensor_msgs::JointState msg;
geometry_msgs::Pose pose;
//std::stringstream ss;
//ss << "hello world " << count;
//msg.data = ss.str();
std::string command;
int command_index;
cout << "Please enter pose value: ";
cin >> command_index;
if(command_index<my_poses.size() && command_index>=0) {
std::cout << "OK command "<<command_index<<std::endl;
MyPose mypose = my_poses[command_index];
std::cout << mypose.x << "," <<mypose.y<<std::endl;
pose.position.x = mypose.x;
pose.position.y = mypose.y;
pose.position.z = mypose.z;
KDL::Rotation rot = KDL::Rotation::RPY(
mypose.roll*M_PI/180.0f,
mypose.pitch*M_PI/180.0f,
mypose.yaw*M_PI/180.0f
);
double qx,qy,qz,qw;
rot.GetQuaternion(qx,qy,qz,qw);
pose.orientation.x = qx;
pose.orientation.y = qy;
pose.orientation.z = qz;
pose.orientation.w = qw;
lar_comau::ComauCommand comau_command;
comau_command.pose = pose;
comau_command.command = "joint";
cartesian_controller.publish(comau_command);
ros::spinOnce();
std::cout << "Published "<<comau_command<<std::endl;
}
}
return 0;
}
bool publishMarkerArray(geometry_msgs::Pose axis, std::vector<geometry_msgs::Pose> trajectoyPoints, std::string refFrameName){
visualization_msgs::MarkerArray tmpMarkerArray;
KDL::Rotation axisR = KDL::Rotation::Quaternion(axis.orientation.x, axis.orientation.y,axis.orientation.z,axis.orientation.w);
KDL::Rotation deltaR = KDL::Rotation::Quaternion(0.0,std::sin(-PI/4),0.0,std::cos(-PI/4));
KDL::Rotation markerR = axisR*deltaR;
visualization_msgs::Marker axisMarker;
axisMarker.header.frame_id = refFrameName;
axisMarker.header.stamp = ros::Time();
axisMarker.ns = "articulationNS";
axisMarker.id = 0;
axisMarker.type = visualization_msgs::Marker::ARROW;
axisMarker.action = visualization_msgs::Marker::ADD;
axisMarker.scale.x = 2;
axisMarker.scale.y = 2;
axisMarker.scale.z = 2;
axisMarker.color.a = 1.0;
axisMarker.color.r = 0.0;
axisMarker.color.g = 1.0;
axisMarker.color.b = 0.0;
//std::cout << "Before: "<<axisMarker.pose.orientation.x << " " << axisMarker.pose.orientation.y << " " << axisMarker.pose.orientation.z << " " << axisMarker.pose.orientation.w << std::endl;
markerR.GetQuaternion(axisMarker.pose.orientation.x,axisMarker.pose.orientation.y,axisMarker.pose.orientation.z,axisMarker.pose.orientation.w);
//std::cout << "After: "<<axisMarker.pose.orientation.x << " " << axisMarker.pose.orientation.y << " " << axisMarker.pose.orientation.z << " " << axisMarker.pose.orientation.w << std::endl;
visualization_msgs::Marker axisText;
axisText.header.frame_id = refFrameName;
axisText.header.stamp = ros::Time();
axisText.ns = "articulationNS";
axisText.id = 1;
axisText.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
axisText.action = visualization_msgs::Marker::ADD;
axisText.scale.x = 0.5;
axisText.scale.y = 0.5;
axisText.scale.z = 0.5;
axisText.color.a = 1.0;
axisText.color.r = 1.0;
axisText.color.g = 1.0;
axisText.color.b = 1.0;
visualization_msgs::Marker trajPoint;
trajPoint.header.frame_id = refFrameName;
trajPoint.header.stamp = ros::Time();
trajPoint.ns = "articulationNS";
trajPoint.type = visualization_msgs::Marker::SPHERE;
trajPoint.action = visualization_msgs::Marker::ADD;
trajPoint.scale.x = .3;
trajPoint.scale.y = .3;
trajPoint.scale.z = .3;
trajPoint.color.a = 1.0;
trajPoint.color.r = 0.0;
trajPoint.color.g = 1.0;
trajPoint.color.b = 0.0;
axisText.pose = axis;
//std::stringstream s;
//s << "(" << axisText.pose.position.x << ", " << axisText.pose.position.y << ")" ;
axisText.text = "Axis of Rotation";
tmpMarkerArray.markers.push_back(axisMarker);
tmpMarkerArray.markers.push_back(axisText);
for(int i=0; i<(int)trajectoyPoints.size(); i++){
trajPoint.pose = trajectoyPoints[i];
trajPoint.id = 2 + i;
tmpMarkerArray.markers.push_back(trajPoint);
}
ROS_INFO("Publishing MarkerArray: Size=%d",(int)tmpMarkerArray.markers.size());
vis_pub.publish( tmpMarkerArray );
return true;
}
bool CartesianGenerator::generateNewVelocityProfiles(RTT::base::PortInterface* portInterface)
{
m_time_passed = os::TimeService::Instance()->secondsSince(m_time_begin);
geometry_msgs::Pose pose;
cmdCartPose.read(pose);
desired_pose = pose;
#if 1
std::cout << "A new pose arrived" << std::endl;
std::cout << "position: " << pose.position.x << " " << pose.position.y << " " << pose.position.z << std::endl;
#endif
m_traject_end.p.x(pose.position.x);
m_traject_end.p.y(pose.position.y);
m_traject_end.p.z(pose.position.z);
m_traject_end.M = Rotation::Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w);
// get current position
geometry_msgs::Pose pose_meas;
m_position_meas_port.read(pose_meas);
m_traject_begin.p.x(pose_meas.position.x);
m_traject_begin.p.y(pose_meas.position.y);
m_traject_begin.p.z(pose_meas.position.z);
m_traject_begin.M = Rotation::Quaternion(pose_meas.orientation.x, pose_meas.orientation.y, pose_meas.orientation.z,
pose_meas.orientation.w);
xi = pose_meas.position.x;
yi = pose_meas.position.y;
zi = pose_meas.position.z;
xf = pose.position.x;
yf = pose.position.y;
zf = pose.position.z;
TrajVectorMagnitude = sqrt((xf - xi) * (xf - xi) + (yf - yi) * (yf - yi) + (zf - zi) * (zf - zi));
TrajVectorDirection.x = (xf - xi) / TrajVectorMagnitude;
TrajVectorDirection.y = (yf - yi) / TrajVectorMagnitude;
TrajVectorDirection.z = (zf - zi) / TrajVectorMagnitude;
double tx, ty, tz;
tx = abs(xf - xi) / m_maximum_velocity[0];
ty = abs(yf - yi) / m_maximum_velocity[0];
tz = abs(zf - zi) / m_maximum_velocity[0];
t_sync = TrajVectorMagnitude / m_maximum_velocity[0];
KDL::Rotation errorRotation = (m_traject_end.M) * (m_traject_begin.M.Inverse());
double x, y, z, w;
errorRotation.GetQuaternion(x, y, z, w);
Eigen::AngleAxis<double> aa;
aa = Eigen::Quaterniond(w, x, y, z);
currentRotationalAxis = aa.axis();
deltaTheta = aa.angle();
theta_vel = deltaTheta / t_sync;
cout << "[CG::GenerateProfiles]:" << endl;
m_time_begin = os::TimeService::Instance()->getTicks();
m_time_passed = 0;
return true;
}
void quaternionKDLToTF(const KDL::Rotation &k, tf::Quaternion &t)
{
double x, y, z, w;
k.GetQuaternion(x, y, z, w);
t = tf::Quaternion(x, y, z, w);
}
