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); }