20 #include <sys/socket.h> 21 #include <linux/sockios.h> 22 #include <linux/can.h> 26 #include "nav_msgs/Odometry.h" 55 geometry_msgs::TransformStamped odom_trans;
57 odom_trans.header.stamp = current_time;
58 odom_trans.header.frame_id =
"odom";
59 odom_trans.child_frame_id =
"base_link";
61 odom_trans.transform.translation.x =
state->
getX()/1000.0;
62 odom_trans.transform.translation.y =
state->
getY()/1000.0;
63 odom_trans.transform.translation.z = 0.0;
64 odom_trans.transform.rotation = odom_quat;
76 nav_msgs::Odometry odom;
78 odom.header.stamp = current_time;
79 odom.header.frame_id =
"odom";
82 odom.pose.pose.position.x =
state->
getX()/1000.0;
83 odom.pose.pose.position.y =
state->
getY()/1000.0;
84 odom.pose.pose.position.z = 0.0;
85 odom.pose.pose.orientation = odom_quat;
88 odom.child_frame_id =
"base_link";
89 odom.twist.twist.linear.x =
state->
getVX()/1000.0;
90 odom.twist.twist.linear.y =
state->
getVY()/1000.0;
105 ROS_ERROR(
"CanListener: mild_base_driving raw socket read, status %zu (%i)", nbytes, errno);
109 else if (nbytes < (
int)
sizeof(
struct can_frame))
111 ROS_ERROR(
"CanListener: read: incomplete CAN frame of size %zu",nbytes);
119 int max_encoder = 0xffff;
120 if (fabs(ticks - ticks_old) > 0.5*max_encoder)
123 ROS_DEBUG(
"CanListener: Overflow left. Old ticks_old = %i", ticks_old);
124 if (ticks > ticks_old)
126 ticks_old = ticks_old + max_encoder;
128 else ticks_old = ticks_old - max_encoder;
129 ROS_DEBUG(
"CanListener: Overflow left. New ticks_old = %i", ticks_old);
136 double velocity = d / d_time;
146 ROS_DEBUG(
"CanListener: Moving forward. velocity = %f", velocity);
160 double radius = d / t;
180 ROS_DEBUG(
"CanListener: Other movement. radius = %f", radius);
186 velocity_average[
counter]= velocity;
198 sum += velocity_average[i];
211 double d = 0, d_left = 0, d_right = 0, t =0 ;
213 double velocity_left = 0, velocity_right = 0;
214 int ticks_left = 0, ticks_right = 0, ticks_left_old = 0, ticks_right_old = 0;
218 ros::Time current_time, last_time, start_time;
226 double d_time = (current_time - last_time).toNSec();
232 ticks_left = (
frame.data[3]<<8)+
frame.data[2];
233 ticks_right = (
frame.data[1]<<8)+
frame.data[0];
235 ROS_DEBUG(
"CanListener: ticks_left: %i, ticks_right: %i", ticks_left,ticks_right);
238 ROS_INFO(
"CanListener: Started successfully.\n");
239 ticks_left_old = ticks_left;
240 ticks_right_old = ticks_right;
258 velocity_left = d_left / d_time;
259 velocity_right = d_right / d_time;
262 ticks_left_old = ticks_left;
263 ticks_right_old = ticks_right;
268 d = ( d_left + d_right ) / 2 ;
269 ROS_DEBUG(
"CanListener: Driven dinstance = %f", d);
272 ROS_DEBUG(
"CanListener: Orientation change = %f", t);
279 if(velocity_left == velocity_right)
286 if(velocity_left == -velocity_right)
308 last_time = current_time;
void sendTransformOdomTF(geometry_msgs::TransformStamped odom_trans)
void publish(const boost::shared_ptr< M > &message) const
void otherMovement(double d, double t, double d_time)
double right_velocity_average[50]
int overflowDetection(int ticks, int ticks_old)
double impulses_per_mm_left
void turningInPlace(double t, double d_time)
nav_msgs::Odometry getOdomMsg(ros::Time current_time)
double get_velocity_left()
double calculateAverage(double velocity_average[], double velocity)
double impulses_per_mm_right
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
void movingForward(double d, double d_time)
geometry_msgs::TransformStamped getOdomTF(ros::Time current_time)
double left_velocity_average[50]
double get_velocity_right()