2 import roslib; roslib.load_manifest('roboint')
5 from math import sin, cos, pi
6 from geometry_msgs.msg import Twist, TransformStamped, Point32
7 from sensor_msgs.msg import PointCloud
8 from nav_msgs.msg import Odometry
9 from roboint.msg import Motor
10 from roboint.msg import Inputs
15 rospy.init_node('robo_explorer')
17 rospy.Subscriber("cmd_vel", Twist, self.cmdVelReceived)
18 rospy.Subscriber("ft/get_inputs", Inputs, self.inputsReceived)
20 self.pub_motor = rospy.Publisher("ft/set_motor", Motor)
21 self.pub_cloud = rospy.Publisher("point_cloud", PointCloud)
22 self.pub_odom = rospy.Publisher("odom", Odometry)
24 self.wheel_dist = 0.188 # 18.8cm
25 self.wheel_size = 0.052*0.5 # 5.1cm gear ration=0.5
31 self.odom_broadcaster = tf.TransformBroadcaster()
32 self.last_time = rospy.Time.now()
36 def inputsReceived(self, msg):
37 current_time = rospy.Time.now()
39 dt = (current_time - self.last_time).to_sec();
40 in_now = msg.input[1:3]
41 in_diff = [abs(a - b) for a, b in zip(in_now, self.last_in)] # get changed inputs
43 in_diff[0] = -in_diff[0]
45 in_diff[1] = -in_diff[1]
47 dist_dir = (in_diff[1] - in_diff[0])*self.wheel_size*pi/8
48 delta_alpha = dist_dir/self.wheel_dist
50 dist = (in_diff[0] + in_diff[1])/2.0*self.wheel_size*pi/8
52 delta_x = cos(self.alpha + delta_alpha/2)*dist
53 delta_y = sin(self.alpha + delta_alpha/2)*dist
55 self.alpha += delta_alpha
58 elif self.alpha < -2*pi:
66 valpha = delta_alpha / dt
68 # since all odometry is 6DOF we'll need a quaternion created from yaw
69 odom_quat = tf.transformations.quaternion_from_euler(0, 0, self.alpha)
71 # first, we'll publish the transform over tf
72 self.odom_broadcaster.sendTransform((0.0, 0.0, 0.0), odom_quat, current_time, "odom", "base_link");
74 # next, we'll publish the odometry message over ROS
76 odom.header.stamp = current_time
77 odom.header.frame_id = "odom"
80 odom.pose.pose.position.x = self.x
81 odom.pose.pose.position.y = self.y
82 odom.pose.pose.position.z = 0.0
83 odom.pose.pose.orientation = odom_quat
86 odom.child_frame_id = "base_link";
87 odom.twist.twist.linear.x = vx
88 odom.twist.twist.linear.y = vy
89 odom.twist.twist.angular.z = valpha
92 self.pub_odom.publish(odom)
96 cloud.header.stamp = current_time
97 cloud.header.frame_id = "sensor_frame"
98 cloud.points.append(Point32(msg.d1/10.0, 0, 0))
99 self.pub_cloud.publish(cloud)
101 self.last_time = current_time
102 self.last_in = in_now
105 def cmdVelReceived(self, msg):
111 # max rot = 0.29rad/s
113 speed_l = int(trans*7/0.1 - rot*7/0.29)
114 speed_r = int(trans*7/0.1 + rot*7/0.29)
115 if speed_l < -7: speed_l = -7
116 elif speed_l > 7: speed_l = 7
117 if speed_r < -7: speed_r = -7
118 elif speed_r > 7: speed_r = 7
122 outmsg.speed = speed_l
123 self.pub_motor.publish(outmsg)
127 outmsg.speed = speed_r
128 self.pub_motor.publish(outmsg)
130 self.speed = (speed_l, speed_r)
132 if __name__ == '__main__':