void StepShm(int cntr)
{
if (!status.calibrated)
{
printf("ZLift is not calibrated. Please calibrate and run again. Exiting.\n");
endme(1);
}
SetTimestamp(GetTimestamp()); //Pass back timestamp as a heartbeat
joint_state_g.header.stamp = ros::Time::now();
joint_state_g.position[0] = status.position;
joint_state_g.velocity[0] = status.velocity;
joint_state_g.effort[0] = status.effort;
zlift_publisher_g.publish(joint_state_g);
/*if (cntr % 100 == 0)
{
if (1)
{
printf("********************************\n");
printf("timestamp: %ld\n", status.timestamp);
{
printf("------------------------------\n");
printf("position: %f\n", status.position);
printf("velocity: %f\n", status.velocity);
printf("effort: %f\n", status.effort);
printf("------------------------------\n");
printf("\n");
}
}
}*/
/*cmd.position = 600;
cmd.velocity = 2000;
cmd.stiffness = 1.0;
cmd.control_mode = JOINT_MODE_ROS_THETA_GC;
cmd.smoothing_mode = SMOOTHING_MODE_SLEW;*/
}
void exit_on_error()
{
endme(0);
}
void StepShm(int cntr)
{
SetTimestamp(GetTimestamp()); //Pass back timestamp as a heartbeat
if (!status.calibrated)
{
printf("Omnibase is not calibrated. Please calibrate and run again. Exiting.\n");
endme(1);
}
odom_g.header.stamp = ros::Time::now();
// get from status
double x = status.x;
double y = status.y;
double th = status.yaw;
double vx = status.x_dot;
double vy = status.y_dot;
double vth = status.yaw_dot;
//ROS_INFO("[STATUS] x,y,th:[%f,%f,%f]",x,y,th);
// get from status
//since all odometry is 6DOF we'll need a quaternion created from yaw
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
//first, we'll publish the transform over tf
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = ros::Time::now();
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";
odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;
//send the transform
//odom_broadcaster.sendTransform(odom_trans);
odom_g.header.frame_id = "odom";
//set the position
odom_g.pose.pose.position.x = x;
odom_g.pose.pose.position.y = y;
odom_g.pose.pose.position.z = 0.0;
odom_g.pose.pose.orientation = odom_quat;
//set the velocity
odom_g.child_frame_id = "base_link";
odom_g.twist.twist.linear.x = vx;
odom_g.twist.twist.linear.y = vy;
odom_g.twist.twist.angular.z = vth;
odom_publisher_g.publish(odom_g);
if (status.timestamp - last_cmd_ts > VEL_TIMEOUT_SEC * 1000000.0)
{
cmd.x_velocity = 0.;
cmd.y_velocity = 0.;
cmd.yaw_velocity = 0.;
}
/* if (cntr % 100 == 0)
{
if (1)
{
printf("********************************\n");
printf("timestamp: %ld\n", (status.timestamp - last_cmd_ts)/1000000);
//printf("to: %ld\n", VEL_TIMEOUT_NS);
{
//printf("JOINT %d\n", i);
printf("------------------------------\n");
printf("X: %f\n",status.x);
printf("Y: %f\n", status.y);
printf("YAW: %f\n", status.yaw);
printf("Vx: %f\n", odom_g.twist.twist.linear.x);
printf("Vy: %f\n", odom_g.twist.twist.linear.y);
printf("Va: %f\n", odom_g.twist.twist.angular.z);
printf("------------------------------\n");
printf("\n");
}
}
}
if (cntr % 100 == 0)
{
if (1)
{
printf("********************************\n");
printf("timestamp: %ld\n", status.timestamp);
{
//printf("JOINT %d\n", i);
printf("------------------------------\n");
printf("X: %f\n", odom_g.pose.pose.position.x);
printf("Y: %f\n", odom_g.pose.pose.position.y);
printf("YAW: %f\n", th);
printf("Vx: %f\n", odom_g.twist.twist.linear.x);
printf("Vy: %f\n", odom_g.twist.twist.linear.y);
printf("Va: %f\n", odom_g.twist.twist.angular.z);
printf("------------------------------\n");
printf("\n");
}
}
}*/
}
