int main(int argc, char* argv[])
{
///ROS Initializations
ros::init(argc, argv, "Roboteq_Channel_Tests");
ros::NodeHandle n;
ros::Publisher publisher = n.advertise<nav_msgs::Odometry>("/encoder_data", 1);
ros::Rate loop_rate(20);
///ROBOTEQ Connection
ROS_INFO("\n\n--- Roboteq Motor Controller Request Gateway Server ---\n");
ROS_INFO("Initializing...");
usleep(500000);
status = device.Connect("/dev/ttyACM0");
while (status != RQ_SUCCESS && ros::ok())
{
ROS_ERROR("Error connecting to device: %d\n", status);
ROS_INFO("Attempting server restart...");
usleep(999999);
device.Disconnect();
status = device.Connect("/dev/ttyAMC0");
if (status == 0) {
ROS_INFO("Connection re-established...");
}
}
///Initialization and Fixed Data
output.pose.pose.position.x=0; output.pose.pose.position.y=0; output.pose.pose.position.z=0;
output.pose.pose.orientation.x=1; output.pose.pose.orientation.y=0; output.pose.pose.orientation.z=0; output.pose.pose.orientation.w=0;
for(int i=0; i<36; i++){
if((i/6)==(i%6)) output.pose.covariance[i]=99999;
else output.pose.covariance[i]=0;
}
output.twist.twist.linear.x = 0; output.twist.twist.linear.y = 0; output.twist.twist.linear.z = 0;
output.twist.twist.angular.x = 0; output.twist.twist.angular.y = 0; output.twist.twist.angular.y = 0;
for(int i=0; i<36; i++){
if((i/6)==(i%6)) output.twist.covariance[i]=99999;
else output.twist.covariance[i]=0;
}
///The Publishing Loop
device.SetCommand(_C, 1, 0);
device.SetCommand(_C, 2, 0);
int ch1_rpm_value = 0, ch2_rpm_value = 0;
localization::roboteq_msg unfiltered;
output.header.frame_id = "odom_combined";
output.child_frame_id = "base_footprint";
while(ros::ok()){
/*Gathering Data from Device*/
device.GetValue(_S, 1, ch1_rpm_value);
device.GetValue(_S, 2, ch2_rpm_value);
/*Preparing Data for Low-Pass Filter*/
//Time
unfiltered.header.stamp = ros::Time::now();
//RPM Stuff
unfiltered.rpm_1 = ch1_rpm_value; unfiltered.rpm_2 = ch2_rpm_value;
/*LowPass Filtering*/
low_pass(unfiltered);
/*Calculating 'v' and 'w' From Filtered Data*/
//Calculate 'v'
v = v_factor*(filtered.rpm_2 - filtered.rpm_1); //rpm_1 is negatived because when the wheel turns forwards, rpm_1 is negative
//Calculate 'w'
w = w_factor*(filtered.rpm_2 + filtered.rpm_1); //positive is left, negative is right (rpm_2 refers to right wheel)
/*Data to Output Message*/
//Header Data
output.header.stamp = filtered.header.stamp;
output.header.seq++;
//Twist Data
output.twist.twist.linear.x = v;
output.twist.twist.angular.z = w;
//Covariances
output.twist.covariance[0] = linearVelocityVariance;
output.twist.covariance[35] = angularVelocityVariance;
/*Publishing*/
publisher.publish(output);
loop_rate.sleep();
}
return 0;
}
