void ATTCallback (IvyClientPtr app, void* data , int argc, char **argv)
{
double att_unit_coef= 0.0139882;
double phi, theta, yaw;
sscanf(argv[0],"%lf %lf %lf %*d %*d %*d",&phi,&theta,&yaw);
phi*=att_unit_coef*DEG2RAD;
theta*=-att_unit_coef*DEG2RAD;
yaw*=-att_unit_coef*DEG2RAD;
q.setRPY(phi,theta,yaw);
//ROS_INFO("Phi %f; Theta %f; Yaw %f", phi,theta,yaw);
//ROS_INFO("q1 %f; q2 %f; q3 %f; q4 %f", q.x(),q.y(),q.z(),q.w());
imu_data.header.stamp = ros::Time::now();
imu_data.orientation.x=q.x();
imu_data.orientation.y=q.y();
imu_data.orientation.z=q.z();
imu_data.orientation.w=q.w();
imu_message.publish(imu_data);
//Only temporary until the rates are equal
att_data.header.stamp = ros::Time::now();
att_data.orientation.x=q.x();
att_data.orientation.y=q.y();
att_data.orientation.z=q.z();
att_data.orientation.w=q.w();
att_message.publish(att_data);
}
tf::Matrix3x3 JPCT_Math::quatToMatrix(tf::Quaternion q) {
tf::Matrix3x3 matrix;
float norm = magnitudeSquared(q);
float s = (double)norm > 0.0?2.0f / norm:0.0F;
float xs = q.x() * s;
float ys = q.y() * s;
float zs = q.z() * s;
float xx = q.x() * xs;
float xy = q.x() * ys;
float xz = q.x() * zs;
float xw = q.w() * xs;
float yy = q.y() * ys;
float yz = q.y() * zs;
float yw = q.w() * ys;
float zz = q.z() * zs;
float zw = q.w() * zs;
matrix[0][0] = 1.0F - (yy + zz);
matrix[1][0] = xy - zw;
matrix[2][0] = xz + yw;
matrix[0][1] = xy + zw;
matrix[1][1] = 1.0F - (xx + zz);
matrix[2][1] = yz - xw;
matrix[0][2] = xz - yw;
matrix[1][2] = yz + xw;
matrix[2][2] = 1.0F - (xx + yy);
return matrix;
}
void Hand_filter::update(tf::Vector3 p, tf::Quaternion& q){
if(b_first){
p_filter_buffer.push_back(p);
q_filter_buffer.push_back(q);
if(p_filter_buffer.size() == p_filter_buffer.capacity()){
b_first = false;
ROS_INFO("====== hand filter ======");
// ROS_INFO("buffer full: %d",p_filter_buffer.size());
ROS_INFO("p: %f %f %f",p.x(),p.y(),p.z());
ROS_INFO("q: %f %f %f %f",q.x(),q.y(),q.z(),q.w());
k_position(0) = p.x();k_position(1) = p.y(); k_position(2) = p.z();
kalman_filter.Init(k_position);
q_tmp = q;
p_tmp = p;
}
}else{
/// Orientation filter
if(jumped(q,q_tmp,q_threashold)){
ROS_INFO("q jumped !");
q = q_tmp;
}
q_attractor(q,up);
q = q_tmp.slerp(q,0.1);
/// Position filter
if(!jumped(p,p_tmp,p_threashold)){
k_measurement(0) = p.x();
k_measurement(1) = p.y();
k_measurement(2) = p.z();
}else{
ROS_INFO("p jumped !");
k_measurement(0) = p_tmp.x();
k_measurement(1) = p_tmp.y();
k_measurement(2) = p_tmp.z();
}
kalman_filter.Update(k_measurement,0.001);
kalman_filter.GetPosition(k_position);
p.setValue(k_position(0),k_position(1),k_position(2));
q_tmp = q;
p_tmp = p;
}
}
/**
* Convert tf::Quaternion to string
*/
template<> std::string toString<tf::Quaternion>(const tf::Quaternion& p_quat)
{
std::stringstream ss;
ss << "(" << p_quat.x() << ", " << p_quat.y() << ", " << p_quat.z() << ", " << p_quat.w() << ")";
return ss.str();
}
/**
* Packs current state in a odom message. Needs a quaternion for conversion.
*/
void pack_pose(tf::Quaternion& q, nav_msgs::Odometry& odom)
{
q.setRPY(0, 0, _theta);
odom.header.stamp = ros::Time::now();
odom.pose.pose.position.x = _x;
odom.pose.pose.position.y = _y;
odom.pose.pose.orientation.x = q.x();
odom.pose.pose.orientation.y = q.y();
odom.pose.pose.orientation.z = q.z();
odom.pose.pose.orientation.w = q.w();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "test2d");
ros::NodeHandle node;
tf::TransformListener t(ros::Duration(20));
tf::StampedTransform tr_o, tr_i;
double a_test(0);
double b_test(0);
double theta_test(0);
double nu_theta(1);
double nu_trans(1);
ROS_INFO_STREAM("waiting for initial transforms");
while (node.ok())
{
ros::Time now(ros::Time::now());
//ROS_INFO_STREAM(now);
if (t.waitForTransform(baseLinkFrame, now, baseLinkFrame, now, odomFrame, ros::Duration(0.1)))
break;
//ROS_INFO("wait");
//ros::Duration(0.1).sleep();
}
ROS_INFO_STREAM("got first odom to baseLink");
while (node.ok())
{
ros::Time now(ros::Time::now());
//ROS_INFO_STREAM(now);
if (t.waitForTransform(kinectFrame, now, kinectFrame, now, worldFrame, ros::Duration(0.1)))
break;
//ROS_INFO("wait");
//ros::Duration(0.1).sleep();
}
ROS_INFO_STREAM("got first world to kinect");
sleep(10);
ros::Rate rate(0.5);
while (node.ok())
{
// sleep
rate.sleep();
// get parameters from transforms
ros::Time curTime(ros::Time::now());
ros::Time lastTime = curTime - ros::Duration(10);
//ROS_INFO_STREAM("curTime: " << curTime << ", lastTime: " << lastTime);
t.waitForTransform(baseLinkFrame, curTime, baseLinkFrame, lastTime, odomFrame, ros::Duration(3));
t.waitForTransform(kinectFrame, curTime, kinectFrame, lastTime, worldFrame, ros::Duration(3));
t.lookupTransform(baseLinkFrame, curTime, baseLinkFrame, lastTime, odomFrame, tr_o);
//ROS_INFO_STREAM("odom to baselink: trans: " << tr_o.getOrigin() << ", rot: " << tr_o.getRotation());
const double alpha_o_tf = tr_o.getOrigin().x();
const double beta_o_tf = tr_o.getOrigin().y();
const double theta_o = 2*atan2(tr_o.getRotation().z(), tr_o.getRotation().w());
const double alpha_o = cos(theta_o)*alpha_o_tf + sin(theta_o)*beta_o_tf;
const double beta_o = -sin(theta_o)*alpha_o_tf + cos(theta_o)*beta_o_tf;
t.lookupTransform(kinectFrame, curTime, kinectFrame, lastTime, worldFrame, tr_i);
//ROS_INFO_STREAM("world to kinect: trans: " << tr_i.getOrigin() << ", rot: " << tr_i.getRotation());
const double alpha_i_tf = tr_i.getOrigin().z();
const double beta_i_tf = -tr_i.getOrigin().x();
const double theta_i = 2*atan2(-tr_i.getRotation().y(), tr_i.getRotation().w());
const double alpha_i = cos(theta_i)*alpha_i_tf + sin(theta_i)*beta_i_tf;
const double beta_i = -sin(theta_i)*alpha_i_tf + cos(theta_i)*beta_i_tf;
lastTime = curTime;
ROS_WARN_STREAM("Input odom: ("<<alpha_o<<", "<<beta_o<<", "<<theta_o<<"), icp: ("\
<<alpha_i<<", "<<beta_i<<", "<<theta_i<<")");
if (abs(theta_i-theta_o) > max_diff_angle)
{
ROS_WARN_STREAM("Angle difference too big: " << abs(theta_i-theta_o));
continue;
}
// compute correspondances, check values to prevent numerical instabilities
const double R_denom = 2 * sin(theta_o);
/*if (abs(R_denom) < limit_low)
{
ROS_WARN_STREAM("magnitude of R_denom too low: " << R_denom);
continue;
}*/
const double kr_1 = (alpha_o * cos(theta_o) + alpha_o + beta_o * sin(theta_o));
const double kr_2 = (beta_o * cos(theta_o) + beta_o - alpha_o * sin(theta_o));
const double phi = atan2(kr_2, kr_1);
const double r_1 = kr_1/R_denom;
const double r_2 = kr_2/R_denom;
const double R = sqrt(r_1*r_1 + r_2*r_2);
const double kC_1 = (beta_i + beta_i * cos(theta_o) + alpha_i * sin(theta_o));
const double kC_2 = (alpha_i + alpha_i * cos(theta_o) + beta_i * sin(theta_o));
//const double xi = atan2(kC_2 + R_denom*b_test, kC_1 + R_denom*a_test);
const double C_1 = kC_1 / R_denom;
const double C_2 = kC_2 / R_denom;
double xi(0);
if (R_denom)
xi = atan2(C_1 + a_test, C_2 + b_test);
else
xi = atan2(kC_1, kC_2);
// compute new values
//double tmp_theta = M_PI/2 - phi - xi;
double tmp_theta = xi - phi;
double tmp_a = R * sin(tmp_theta+phi) - C_1;
double tmp_b = R * cos(tmp_theta+phi) - C_2;
//tmp_theta = (tmp_theta<-M_PI)?tmp_theta+2*M_PI:((tmp_theta>M_PI)?tmp_theta-2*M_PI:tmp_theta);
//const double V = sqrt((C_1+a_test)*(C_1+a_test)+(C_2+b_test)*(C_2+b_test));
/*const double err = sqrt((a_test-tmp_a)*(a_test-tmp_a)+(b_test-tmp_b)*(b_test-tmp_b));
const double err_pred = sqrt(R*R + V*V -2*R*V*cos(tmp_theta+phi-xi));
if (abs(err-err_pred)>0.00001)
{
ROS_WARN_STREAM("Error="<<err<<" Computed="<<err_pred);
ROS_WARN_STREAM("chosen: ("<<tmp_a<<", "<<tmp_b<<"); rejected: ("
<<R*sin(tmp_theta+phi+M_PI)-C_1<<", "<<R*cos(tmp_theta+phi+M_PI)-C_2<<")");
ROS_WARN_STREAM("R: "<<R<<", V: "<<V<<", C_1: "<<C_1<<", C_2: "<<C_2\
<<", phi: "<<phi<<", xi: "<<xi<<", theta: "<<tmp_theta);
}*/
if (R>min_R_rot)
{
theta_test = atan2((1-nu_theta)*sin(theta_test)+nu_theta*sin(tmp_theta),
(1-nu_theta)*cos(theta_test)+nu_theta*cos(tmp_theta));
nu_theta = max(min_nu, 1/(1+1/nu_theta));
}
if (R<max_R_trans)
{
a_test = (1-nu_trans)*a_test + nu_trans*tmp_a;
b_test = (1-nu_trans)*b_test + nu_trans*tmp_b;
nu_trans = max(min_nu, 1/(1+1/nu_trans));
}
// compute transform
const tf::Quaternion quat_trans = tf::Quaternion(a_test, b_test, 0, 1);
const tf::Quaternion quat_test = tf::Quaternion(0, 0, sin(theta_test/2), cos(theta_test / 2));
const tf::Quaternion quat_axes = tf::Quaternion(-0.5, 0.5, -0.5, 0.5);
const tf::Quaternion quat_rot = quat_test*quat_axes;
tf::Quaternion quat_tmp = quat_rot.inverse()*quat_trans*quat_rot;
const tf::Vector3 vect_trans = tf::Vector3(quat_tmp.x(), quat_tmp.y(), 0);
tf::Transform transform;
transform.setRotation(quat_rot);
transform.setOrigin(vect_trans);
ROS_INFO_STREAM("Estimated transform: trans: " << a_test << ", " <<
b_test << ", rot: " << 2*atan2(quat_test.z(), quat_test.w()));
static tf::TransformBroadcaster br;
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
baseLinkFrame, myKinectFrame));
}
return 0;
}
int main (int argc, char** argv) {
ros::init(argc, argv, "tp_serial");
ros::NodeHandle n;
ROS_INFO("Serial server is starting");
ros::Time current_time;
tf::TransformBroadcaster transform_broadcaster;
ros::Subscriber ucCommandMsg; // subscript to "cmd_vel" for receive command
ros::Publisher odom_pub; // publish the odometry
ros::Publisher path_pub;
nav_msgs::Odometry move_base_odom;
nav_msgs::Path pathMsg;
static tf::Quaternion move_base_quat;
//Initialize fixed values for odom and tf message
move_base_odom.header.frame_id = "odom";
move_base_odom.child_frame_id = "base_robot";
//Reset all covariance values
move_base_odom.pose.covariance = boost::assign::list_of(WHEEL_COVARIANCE)(0)(0)(0)(0)(0)
(0)(WHEEL_COVARIANCE)(0)(0)(0)(0)
(0)(0)(999999)(0)(0)(0)
(0)(0)(0)(999999)(0)(0)
(0)(0)(0)(0)(999999)(0)
(0)(0)(0)(0)(0)(WHEEL_COVARIANCE);
move_base_odom.twist.covariance = boost::assign::list_of(WHEEL_COVARIANCE)(0)(0)(0)(0)(0)
(0)(WHEEL_COVARIANCE)(0)(0)(0)(0)
(0)(0)(999999)(0)(0)(0)
(0)(0)(0)(999999)(0)(0)
(0)(0)(0)(0)(999999)(0)
(0)(0)(0)(0)(0)(WHEEL_COVARIANCE);
pathMsg.header.frame_id = "odom";
//----------ROS Odometry publishing------------------
//----------Initialize serial connection
ROS_INFO("Serial Initialing:\n Port: %s \n BaudRate: %d", DEFAULT_SERIALPORT, DEFAULT_BAUDRATE);
int fd = -1;
struct termios newtio;
FILE *fpSerial = NULL;
// read/write, not controlling terminal for process
fd = open(DEFAULT_SERIALPORT, O_RDWR | O_NOCTTY | O_NDELAY);
if (fd < 0) {
ROS_ERROR("Serial Init: Could not open serial device %s", DEFAULT_SERIALPORT);
return 0;
}
//set up new setting
memset(&newtio, 0, sizeof(newtio));
newtio.c_cflag = CS8 | CLOCAL | CREAD; //8bit, no parity, 1 stop bit
newtio.c_iflag |= IGNBRK; //ignore break codition
newtio.c_oflag = 0; //all options off
//newtio.c_lflag = ICANON; //process input as lines
newtio.c_cc[VTIME] = 0;
newtio.c_cc[VMIN] = 20; // byte readed per a time
//active new setting
tcflush(fd, TCIFLUSH);
if (cfsetispeed(&newtio, BAUDMACRO) < 0 || cfsetospeed(&newtio, BAUDMACRO)) {
ROS_ERROR("Serial Init: Failed to set serial BaudRate: %d", DEFAULT_BAUDRATE);
close(fd);
return 0;
}
ROS_INFO("Connection established with %s at %d.", DEFAULT_SERIALPORT, BAUDMACRO);
tcsetattr(fd, TCSANOW, &newtio);
tcflush(fd, TCIOFLUSH);
// Open file as a standard I/O stream
fpSerial = fdopen(fd, "r+");
if (!fpSerial) {
ROS_ERROR("Serial Init: Failed to open serial stream %s", DEFAULT_SERIALPORT);
fpSerial = NULL;
}
//Creating message to talk with ROS
//Subscribe to ROS message
ucCommandMsg = n.subscribe<geometry_msgs::Twist>("cmd_vel", 1, ucCommandCallback);
//Setup to publish ROS message
odom_pub = n.advertise<nav_msgs::Odometry>("tp_robot/odom", 10);
path_pub = n.advertise<nav_msgs::Path>("tp_robot/odomPath", 10);
// An "adaptive" timer to maintain periodical communication with the MCU
ros::Rate rate(FPS);
uint8_t i = FPS;
while (i--) {
find_mean = true;
rate.sleep();
ros::spinOnce();
}
find_mean = false;
//Loop for input
while(ros::ok()) {
//Process the callbacks
ros::spinOnce();
//Impose command and get back respone
int res;
cmd_frame[0] = 'f';
//cmd_frame[1] = 'T';
*(uint16_t *)(cmd_frame + 2) = pkg_id;
//*(uint16_t *)(cmd_frame + 4) = left_speed; //set left speed
//*(uint16_t *)(cmd_frame + 6) = right_speed; // set right speed
res = write(fd, cmd_frame, 8);
if (res < 8) ROS_ERROR("Error sending command");
current_time = ros::Time::now();
//sleep to wait the response is returned
rate.sleep();
//Read a number of RCV_LENGHT chars as if they have arrived
res = read(fd, rsp_frame, RCV_LENGTH);
if (res < RCV_LENGTH) {
//ROS_INFO("frame %d dropped", pkg_id);
while(read(fd, rsp_frame, 1) > 0);
}
else {
uint16_t rcv_pkg_id = *(uint16_t *)(rsp_frame +2);
if(rcv_pkg_id == pkg_id) {
//read the commands to display
cmd_field[0] = rsp_frame[0];
cmd_field[1] = rsp_frame[1];
cmd_field[2] = 0;
//Time sent from RTOS to check if the moving base has been reset
uint8_t reset_robot = *(uint8_t *)(rsp_frame + 16);
//Calculate the x,y,z,v,w odometry data
double left_speed = *(int16_t *)(rsp_frame + 4)*M_PI/98500;
double right_speed = *(int16_t *)(rsp_frame + 6)*M_PI/98500;
//Get the newly traveled distance on each wheel
if (rcv_pkg_id == 0 || reset_robot == 1 ) {
left_path_offset = *(int32_t *)(rsp_frame +8)*M_PI/98500;
right_path_offset = *(int32_t *)(rsp_frame + 12)*M_PI/98500;
left_path = 0;
right_path = 0;
}
else {
left_path = *(int32_t *)(rsp_frame +8)*M_PI/98500 - left_path_offset;
right_path = *(int32_t *)(rsp_frame + 12)*M_PI/98500 - right_path_offset;
}
//Calculate and publish the odometry data
double left_delta = left_path - left_path1;
double right_delta = right_path - right_path1;
//only if the robot has traveled a significant distance will be calculate the odometry
if(!((left_delta < 0.0075 && left_delta > -0.0075) && (right_delta < 0.0075 && right_delta > -0.0075))) {
//Calculate the pose in reference to world base
theta += (right_delta - left_delta)/0.3559;
//translate theta to [-pi; pi]
if(theta > (2*M_PI)) theta -= 2*M_PI;
if (theta < 0) theta += 2*M_PI;
//Calculate displacement
double disp = (right_delta + left_delta)/2;
x += disp*cos(theta);
y += disp*sin(theta);
//Calculate the twist in reference to the robot base
linear_x = (right_speed + left_speed)/2;
angular_z = (right_speed - left_speed)/0.362;
left_path1 = left_path;
right_path1 = right_path;
}
// Infer the rotation angle and publish transform
move_base_quat = tf::Quaternion(tf::Vector3(0, 0, 1), theta);
transform_broadcaster.sendTransform(tf::StampedTransform(
tf::Transform(move_base_quat, tf::Vector3(x,y,0)),
current_time, "odom", "base_robot"));
//Publish the odometry message over ROS
move_base_odom.header.stamp = current_time;
//move_base_odom.header.stamp = ros::Time::now();
//set the position
move_base_odom.pose.pose.position.x = x;
move_base_odom.pose.pose.position.y = y;
move_base_odom.pose.pose.position.z = 0;
move_base_odom.pose.pose.orientation.x = move_base_quat.x();
move_base_odom.pose.pose.orientation.y = move_base_quat.y();
move_base_odom.pose.pose.orientation.z = move_base_quat.z();
move_base_odom.pose.pose.orientation.w = move_base_quat.w();
//Set the velocity
move_base_odom.child_frame_id = "base_robot";
move_base_odom.twist.twist.linear.x = linear_x;
move_base_odom.twist.twist.linear.y = 0;
move_base_odom.twist.twist.angular.z = angular_z;
//Push back the pose to path message
pathMsg.header = move_base_odom.header;
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header = move_base_odom.header;
pose_stamped.pose = move_base_odom.pose.pose;
pathMsg.poses.push_back(pose_stamped);
//publish the odom and path message
odom_pub.publish(move_base_odom);
path_pub.publish(pathMsg);
//ROS_INFO("MCU responded: %s\t%d\t%f\t%f\t%f\t%f\t%d", cmd_field,\
rcv_pkg_id,\
left_speed,\
right_speed,\
left_path,\
right_path,\
reset_robot);
}
else {
ROS_INFO("frame %d corrupted !", pkg_id);
while(read(fd, rsp_frame, 1) > 0);
}
}
void Jumps::update(tf::Vector3& origin,tf::Quaternion& orientation){
if(bFirst){
origin_buffer.push_back(origin);
orientation_buffer.push_back(orientation);
if(origin_buffer.size() == origin_buffer.capacity()){
bFirst = false;
ROS_INFO("====== jump filter full ======");
}
}else{
origin_tmp = origin_buffer[origin_buffer.size()-1];
orientation_tmp = orientation_buffer[orientation_buffer.size()-1];
if(bDebug){
std::cout<< "=== jum debug === " << std::endl;
std::cout<< "p : " << origin.x() << "\t" << origin.y() << "\t" << origin.z() << std::endl;
std::cout<< "p_tmp: " << origin_tmp.x() << "\t" << origin_tmp.y() << "\t" << origin_tmp.z() << std::endl;
std::cout<< "p_dis: " << origin.distance(origin_tmp) << std::endl;
std::cout<< "q : " << orientation.x() << "\t" << orientation.y() << "\t" << orientation.z() << "\t" << orientation.w() << std::endl;
std::cout<< "q_tmp: " << orientation_tmp.x() << "\t" << orientation_tmp.y() << "\t" << orientation_tmp.z() << "\t" << orientation_tmp.w() << std::endl;
std::cout<< "q_dis: " << dist(orientation,orientation_tmp) << std::endl;
}
/// Position jump
if(jumped(origin,origin_tmp,origin_threashold)){
ROS_INFO("position jumped !");
origin = origin_tmp;
// exit(0);
}else{
origin_buffer.push_back(origin);
}
/// Orientation jump
if(jumped(orientation,orientation_tmp,orientation_threashold)){
ROS_INFO("orientation jumped !");
orientation = orientation_tmp;
//exit(0);
}else{
orientation_buffer.push_back(orientation);
}
}
}
float JPCT_Math::magnitudeSquared(tf::Quaternion q) {
return q.w() * q.w() + q.x() * q.x() + q.y() * q.y() + q.z() * q.z();
}