def sub_cb(self, msg): if (msg.control.speed <> self.last_speed): JC = JointCommand(); JC.name = 'power_motor'; JC.type = 'speed'; JC.speed = -msg.control.speed*100; #convert m/s to deg/s for Lego NXT_steering self.last_speed = msg.control.speed; self.pub.publish(JC) if (msg.control.steering_angle <> self.last_steering_angle): JC = JointCommand(); JC.name = 'steering_motor'; JC.type = 'to_angle'; JC.angle = msg.control.steering_angle*360/(2*math.pi)*self.ratioGearSteering; JC.speed = 400; self.last_steering_angle = msg.control.steering_angle; self.pub.publish(JC)
def sub_cb(self, msg):
if (msg.control.speed <> self.last_speed):
JC = JointCommand()
JC.name = 'power_motor'
JC.type = 'speed'
JC.speed = -msg.control.speed * 100
#convert m/s to deg/s for Lego NXT_steering
self.last_speed = msg.control.speed
self.pub.publish(JC)
if (msg.control.steering_angle <> self.last_steering_angle):
JC = JointCommand()
JC.name = 'steering_motor'
JC.type = 'to_angle'
JC.angle = msg.control.steering_angle * 360 / (
2 * math.pi) * self.ratioGearSteering
JC.speed = 400
self.last_steering_angle = msg.control.steering_angle
self.pub.publish(JC)
def sub_cb_js(self, msg):
if (msg.name[0] == 'radar_motor'):
#control radar
if ((msg.position[0]*180/math.pi > self.last_angle_cmd - 5 and self.last_angle_cmd>0) or (msg.position[0]*180/math.pi < self.last_angle_cmd + 5 and self.last_angle_cmd<0) or (self.init_radar_motor == False)):
rospy.loginfo('Init command radar');
if (self.init_radar_motor == True):
#publish LaserScan message
self.LS_msg.angle_max = -msg.position[0]/5;
self.LS_msg.angle_increment = (self.LS_msg.angle_max - self.LS_msg.angle_min)/self.count_ray;
self.LS_msg.time_increment = 0.01;
self.pub_LS.publish(self.LS_msg);
self.init_radar_motor = True;
#new LaserScan message
self.count_ray = 0;
self.LS_msg = LaserScan();
self.LS_msg.header.frame_id = "base_link";
self.LS_msg.header.stamp = rospy.Time.now();
self.LS_msg.angle_min = -msg.position[0]/5;
self.LS_msg.range_min = 0.04;
self.LS_msg.range_max = 2.55;
#command for run motor radar
JC = JointCommand();
JC.name = 'radar_motor';
JC.type = 'to_angle';
JC.speed = 300;
self.last_angle_cmd = -self.last_angle_cmd;
JC.angle = self.last_angle_cmd;
self.pub_JC.publish(JC);
self.count_ray = self.count_ray + 1;
angle = msg.position[0]/5;
#add ray to laserscan message
self.LS_msg.ranges.append(self.last_range);
#publish PointCloud message
pnt = PointCloud()
pnt.header.frame_id = "base_link"
pnt.header.stamp = rospy.Time.now()
#rospy.loginfo('Range:'+str(self.last_range)+' Angle: '+str(angle)+' X: '+str(self.last_range*cos(angle))+' Y: '+str(self.last_range*sin(angle)));
pnt.points.append(Point32(self.last_range*cos(angle), -self.last_range*sin(angle), 0))
self.pub_PC.publish(pnt)