2 import roslib; roslib.load_manifest('roboint')
5 from math import sin, cos
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)
29 self.odom_broadcaster = tf.TransformBroadcaster()
30 self.last_time = rospy.Time.now()
34 def inputsReceived(self, msg):
35 current_time = rospy.Time.now()
38 cloud.header.stamp = current_time
39 cloud.header.frame_id = "sensor_frame"
40 cloud.points.append(Point32(msg.d1/10.0, 0, 0))
41 self.pub_cloud.publish(cloud)
43 dt = (current_time - self.last_time).to_sec();
44 in_now = msg.input[1:3]
45 in_diff = [abs(a - b) for a, b in zip(in_now, self.last_in)] # get changed inputs
47 in_diff[0] = -in_diff[0]
49 in_diff[1] = -in_diff[1]
51 self.diff_to_angle = 0.1 # TODO
52 diff_si = in_diff # TODO
54 delta_th = (diff_si[0] - diff_si[1]) * self.diff_to_angle
57 movement = (diff_si[0] + diff_si[1])/2.0
58 delta_x = cos(self.th)*movement
59 delta_y = sin(self.th)*movement
68 # first, we'll publish the transform over tf
69 odom_trans = TransformStamped()
70 odom_trans.header.stamp = current_time
71 odom_trans.header.frame_id = "odom"
72 odom_trans.child_frame_id = "base_link"
73 odom_trans.transform.translation.x = self.x
74 odom_trans.transform.translation.y = self.y
75 odom_trans.transform.translation.z = 0.0
76 odom_trans.transform.rotation = self.th
79 #self.odom_broadcaster.sendTransform(odom_trans);
81 # next, we'll publish the odometry message over ROS
83 odom.header.stamp = current_time
84 odom.header.frame_id = "odom"
87 odom.pose.pose.position.x = self.x
88 odom.pose.pose.position.y = self.y
89 odom.pose.pose.position.z = 0.0
90 odom.pose.pose.orientation = self.th
93 odom.child_frame_id = "base_link";
94 odom.twist.twist.linear.x = vx
95 odom.twist.twist.linear.y = vy
96 odom.twist.twist.angular.z = vth
99 self.pub_odom.publish(odom)
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__':