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();
}
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);
}
}
}
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);
}
}
float JPCT_Math::magnitudeSquared(tf::Quaternion q) {
return q.w() * q.w() + q.x() * q.x() + q.y() * q.y() + q.z() * q.z();
}