Hello,
I am building my own car-like robot. For the driving motor controller, I am using the SDC2130 from Roboteq. I have found this package on github called [roboteq_diff_driver](https://github.com/ecostech/roboteq_diff_driver) which is able to control two motors in a differential drive setup (subscribes to cmd_Vel, publishes to odom and broadcasts odom tf).
I am trying to expand on this to include ackermann steering.
**My setup:**
*Driving*: modify the roboteq_diff_driver package to only listen to the linear velocity so the robot is only either going forward or backward
*Steering*: I am using an arduino to control some [linear actuators](https://s3.amazonaws.com/actuonix/Actuonix+P16+Datasheet.pdf) to steer the wheels. I can subscribe to cmd_vel (more specficially, the angular velocity part) with the arduino and then control the linear actuators to meet the desired angular velocity based on ackermann geometry. So although arduino is steering the wheels correctly based on cmd_vel, I dont know how to connect this with rviz since all rviz sees is the odom tf published from the package, which in this case, is only going forward and backward. This is problematic because I am not able to use the navigation stack properly.
**My question:**
How do I feed steering information back in to rviz (Base_link frame) so that it considers steering?
Would using an IMU solve this problem?
I was thinking, am I able to use an IMU to obtain the yaw data and then use this data instead of calculating the yaw based on differential drive kinematics as demonstrated by the package (see code below) and send this information to base_link?
Can someone let me know if I am on the right track or if there are any alternative way to combine steering with rviz?
Below is the part of the package that handles odom publishing:
void MainNode::odom_publish()
{
// determine delta time in seconds
uint32_t nowtime = millis();
float dt = (float)DELTAT(nowtime,odom_last_time) / 1000.0;
odom_last_time = nowtime;
#ifdef _ODOM_DEBUG
/*
ROS_DEBUG("right: ");
ROS_DEBUG(odom_encoder_right);
ROS_DEBUG(" left: ");
ROS_DEBUG(odom_encoder_left);
ROS_DEBUG(" dt: ");
ROS_DEBUG(dt);
ROS_DEBUG("");
*/
#endif
// determine deltas of distance and angle
float linear = ((float)odom_encoder_right / (float)encoder_cpr * wheel_circumference + (float)odom_encoder_left / (float)encoder_cpr * wheel_circumference) / 2.0;
// float angular = ((float)odom_encoder_right / (float)encoder_cpr * wheel_circumference - (float)odom_encoder_left / (float)encoder_cpr * wheel_circumference) / track_width * -1.0;
float angular = ((float)odom_encoder_right / (float)encoder_cpr * wheel_circumference - (float)odom_encoder_left / (float)encoder_cpr * wheel_circumference) / track_width;
#ifdef _ODOM_DEBUG
/*
ROS_DEBUG("linear: ");
ROS_DEBUG(linear);
ROS_DEBUG(" angular: ");
ROS_DEBUG(angular);
ROS_DEBUG("");
*/
#endif
// Update odometry
odom_x += linear * cos(odom_yaw); // m
odom_y += linear * sin(odom_yaw); // m
odom_yaw = NORMALIZE(odom_yaw + angular); // rad
#ifdef _ODOM_DEBUG
//ROS_DEBUG_STREAM( "odom x: " << odom_x << " y: " << odom_y << " yaw: " << odom_yaw );
#endif
// Calculate velocities
float vx = (odom_x - odom_last_x) / dt;
float vy = (odom_y - odom_last_y) / dt;
float vyaw = (odom_yaw - odom_last_yaw) / dt;
#ifdef _ODOM_DEBUG
//ROS_DEBUG_STREAM( "velocity vx: " << odom_x << " vy: " << odom_y << " vyaw: " << odom_yaw );
#endif
odom_last_x = odom_x;
odom_last_y = odom_y;
odom_last_yaw = odom_yaw;
#ifdef _ODOM_DEBUG
/*
ROS_DEBUG("vx: ");
ROS_DEBUG(vx);
ROS_DEBUG(" vy: ");
ROS_DEBUG(vy);
ROS_DEBUG(" vyaw: ");
ROS_DEBUG(vyaw);
ROS_DEBUG("");
*/
#endif
geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromYaw(odom_yaw);
if ( pub_odom_tf )
{
tf_msg.header.seq++;
tf_msg.header.stamp = ros::Time::now();
tf_msg.transform.translation.x = odom_x;
tf_msg.transform.translation.y = odom_y;
tf_msg.transform.translation.z = 0.0;
tf_msg.transform.rotation = quat;
odom_broadcaster.sendTransform(tf_msg);
}
odom_msg.header.seq++;
odom_msg.header.stamp = ros::Time::now();
odom_msg.pose.pose.position.x = odom_x;
odom_msg.pose.pose.position.y = odom_y;
odom_msg.pose.pose.position.z = 0.0;
odom_msg.pose.pose.orientation = quat;
odom_msg.twist.twist.linear.x = vx;
odom_msg.twist.twist.linear.y = vy;
odom_msg.twist.twist.linear.z = 0.0;
odom_msg.twist.twist.angular.x = 0.0;
odom_msg.twist.twist.angular.y = 0.0;
odom_msg.twist.twist.angular.z = vyaw;
odom_pub.publish(odom_msg);
}
↧