err_x = (self.target_point.linear.x - self.state.pose.pose.position.x) err_y = (self.target_point.linear.y - self.state.pose.pose.position.y) euler_angle = self.quat2eul(self.state.pose.pose.orientation) cmd.linear.x = self.k_p_xy * (err_x * math.cos(euler_angle) + err_y * math.sin(euler_angle)) cmd.linear.y = self.k_p_xy * (- err_x * math.sin(euler_angle) + err_y * math.cos(euler_angle)) cmd.linear.z = self.k_p_z * (self.target_point.linear.z - self.state.pose.pose.position.z) cmd.angular.z = self.k_yaw * (self.target_point.angular.z - euler_angle.z) self.move_cmd_send(cmd) if __name__ == "__main__": rospy.init_node('DroneInterface1') drone_1 = DroneInterface('/uav1/ground_truth/state', '/A/uav1/velocity_cmd') sub_target1 = rospy.Subscriber('/uav1/target_point', drone_1.set_target_point) target = Twist() target.linear.x = 5 target.linear.y = 4 target.linear.z = 2 target.angular.z = 0 while not rospy.is_shutdown(): drone_1.set_target_point(target) drone_1.run()
if __name__=="__main__": rospy.init_node("le_scan") velocidade_saida = rospy.Publisher("/cmd_vel", Twist, queue_size = 3 ) recebe_scan = rospy.Subscriber("/bumper", UInt8, scaneou) while not rospy.is_shutdown(): if bumper == 1: v= -0.10 w = -0.15 elif bumper == 2: v= -0.10 w = 0.15 elif bumper == 3: v= 0.10 w = -0.15 elif bumper == 4: v= 0.10 w = 0.15 if passa: passa = False v=0 w=0 else: v = 0.17 w = 0 velocidade = Twist(Vector3(v, 0, 0), Vector3(0, 0,w)) velocidade_saida.publish(velocidade) rospy.sleep(0.5)
def dynamics_function(self, particle_states): # Get current cmd_vel try: cmd_vel = rospy.wait_for_message("/cmd_vel", Twist, timeout=0.1) except: # If nobody is publishing, set the commands to zero cmd_vel = Twist() cmd_vel.angular.z = 0 cmd_vel.linear.x = 0 # Time step between updates delta_t = 1 / self.update_rate # Calculate change in angle and arc length traversed #TODO: FIGURE OUT WHY THESE CONSTANTS ARE REQUIRED delta_theta = cmd_vel.angular.z * delta_t delta_s = cmd_vel.linear.x * delta_t #print("delta_x: {} \t delta_y: {} \t delta_theta: {}".format(delta_x, delta_y, delta_theta)) # Init variables to store translation in x and y directions in the robot frame # delta_y is the forward direction of the robot # delta_x is the right direction of the robot # NOTE: This coordinate frame does not coincide with the ROS coordinate frame of the robot # ROS coordinate frame is rotated 90 degrees counter-clockwise delta_x_robot = 0.0 delta_y_robot = 0.0 # This is a singularity where the robot only moves forward # Radius of circle is infinite if delta_theta == 0: delta_y_robot = delta_s else: # Radius of the circle along which the robot is travelling r = delta_s / delta_theta # Next, calculate the translation in the robot frame delta_x_robot = r * (np.cos(delta_theta) - 1) delta_y_robot = r * np.sin(delta_theta) for i in range(particle_states.shape[0]): # Transform x and y translation in robot frame to world coordinate frame for each particle # TODO REMOVE HACKY pi/2 fix delta_x = delta_x_robot * np.cos( particle_states[i][2] - np.pi / 2) - delta_y_robot * np.sin(particle_states[i][2] - np.pi / 2) delta_y = delta_x_robot * np.sin( particle_states[i][2] - np.pi / 2) + delta_y_robot * np.cos(particle_states[i][2] - np.pi / 2) particle_states[i][0] = particle_states[i][0] + delta_x particle_states[i][1] = particle_states[i][1] + delta_y particle_states[i][2] = particle_states[i][2] + delta_theta # if(i == 0): # print("delta_x: {} \t delta_y: {} \t delta_theta: {}".format(delta_x_robot, delta_y_robot, delta_theta)) rospy.loginfo("Succesfully completed dynamics function") # Return updated particles return particle_states
def set_cmd_vel(self, position): rospy.loginfo(" goal: [%5.2f, %5.2f]", position.x, position.y) goal_x = round(position.x, 4) goal_y = round(position.y, 4) # Initialize the robot_position variable as a Point type robot_position = Point() # Initialize the command move_cmd = Twist() rospy.loginfo(" now -> %s", ok) # Get the starting position values (robot_position, rotation) = self.get_odom() if (ok == True): # Set the movement command to forward motion move_cmd.linear.x = self.linear_speed distanceToGoal = sqrt( pow((goal_x - robot_position.x), 2) + pow((goal_y - robot_position.y), 2)) while (distanceToGoal > self.distance_tolerance): #and not rospy.is_shutdown(): # Publish the Twist message and sleep 1 cycle self.cmd_vel.publish(move_cmd) rospy.sleep(1) # Get the current position (robot_position, rotation) = self.get_odom() # Compute the Euclidean distance from the start distanceToGoal = sqrt( pow((goal_x - robot_position.x), 2) + pow((goal_y - robot_position.x), 2)) rospy.loginfo( " distanceToGoal:[%5.2f], now -> robot:[%5.2f,%5.2f]", distanceToGoal, robot_position.x, robot_position.y) # Stop the robot before the rotation #Stopping our robot after the movement is over move_cmd.linear.x = 0 move_cmd.angular.z = 0 self.cmd_vel.publish(move_cmd) #rospy.sleep(1) # Set the movement command to a rotation move_cmd.angular.z = self.angular_speed # Track the last angle measured last_angle = rotation # Track how far we have turned turn_angle = 0 goal_angle = atan2(goal_y - robot_position.y, goal_x - robot_position.x) while abs(turn_angle + self.angular_tolerance) < abs( goal_angle): #and not rospy.is_shutdown(): # Publish the Twist message and sleep 1 cycle self.cmd_vel.publish(move_cmd) rospy.sleep(1) # Get the current rotation (robot_position, rotation) = self.get_odom() # Compute the amount of rotation since the last loop delta_angle = normalize_angle(rotation - last_angle) # Add to the running total turn_angle += delta_angle last_angle = rotation # Stop the robot before the next leg move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1) # Stop the robot for good self.cmd_vel.publish(Twist())
def callback_feedback(data): ''' Applies adaptive PID to velcity input from odom readings and publishes. :params data [Odometry] :params output [Twist] :params plot [Twist] ''' global active_vel global tar_vel global tar_omega global wheelbase global error_sum global error global error_diff global output global i global flag global kp global ki global kd global pub global prev_error global gear_stat global acc_thershold global brake_threshold global act_velocity global yp global yi global yd siny = 2.0 * (data.pose.pose.orientation.w * data.pose.pose.orientation.z + data.pose.pose.orientation.x * data.pose.pose.orientation.y) cosy = 1.0 - 2.0 * ( data.pose.pose.orientation.y * data.pose.pose.orientation.y + data.pose.pose.orientation.z * data.pose.pose.orientation.z) yaw = math.atan2(siny, cosy) last_recorded_vel = (data.twist.twist.linear.x * math.cos(yaw) + data.twist.twist.linear.y * math.sin(yaw)) active_vel = last_recorded_vel plot = Twist() output = Twist() error = tar_vel - active_vel error_sum += error error_diff = error - prev_error prev_error = error if error == 0: if tar_vel == 0: output.linear.x = 0 else: output.linear.x = output.linear.x - 5 # updating kp, ki, kd using MIT rule kp = kp + yp * error * error ki = ki + yi * error * error_sum kd = kd + yd * error * error_diff rospy.loginfo("kp is : %f", kp) rospy.loginfo("ki is : %f", ki) rospy.loginfo("kd is : %f", kd) # PID on velocity with updated parameters if error > 0.01: output.linear.x = (kp * error + ki * error_sum + kd * error_diff) if error < -0.01: output.linear.x = ((kp * error + ki * error_sum + kd * error_diff) - brake_threshold) plot.linear.x = tar_vel plot.linear.y = active_vel plot.linear.z = tar_vel - active_vel # error term # thresholding the forward velocity if output.linear.x > 100: output.linear.x = 100 if output.linear.x < -100: output.linear.x = -100 # thresholding the angle output.angular.z = min(30.0, max(-30.0, tar_delta)) rospy.loginfo("linear velocity : %f", plot.linear.y) rospy.loginfo("target linear velocity : %f", plot.linear.x) rospy.loginfo("delta : %f", output.angular.z) prius_pub(output) pub1.publish(plot)
def __init__(self): # Give the node a name rospy.init_node('out_and_back', anonymous=False) # Set rospy to execute a shutdown function when exiting rospy.on_shutdown(self.shutdown) # Publisher to control the robot's speed self.cmd_vel = rospy.Publisher('/cmd_vel_mux/input/navi', Twist, queue_size=5) # How fast will we update the robot's movement? rate = 20 # Set the equivalent ROS rate variable r = rospy.Rate(rate) # Set the forward linear speed to 0.15 meters per second linear_speed = 0.15 # Set the travel distance in meters goal_distance = 1.0 # Set the rotation speed in radians per second angular_speed = 0.5 # Set the angular tolerance in degrees converted to radians angular_tolerance = radians(1.0) # Set the rotation angle to Pi radians (180 degrees) goal_angle = pi # Initialize the tf listener self.tf_listener = tf.TransformListener() # Give tf some time to fill its buffer rospy.sleep(2) # Set the odom frame self.odom_frame = '/odom' # Find out if the robot uses /base_link or /base_footprint try: self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_footprint' except (tf.Exception, tf.ConnectivityException, tf.LookupException): try: self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_link' except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint") rospy.signal_shutdown("tf Exception") # Initialize the position variable as a Point type position = Point() # Loop once for each leg of the trip for i in range(2): # Initialize the movement command move_cmd = Twist() # Set the movement command to forward motion move_cmd.linear.x = linear_speed # Get the starting position values (position, rotation) = self.get_odom() x_start = position.x y_start = position.y # Keep track of the distance traveled distance = 0 # Enter the loop to move along a side while distance < goal_distance and not rospy.is_shutdown(): # Publish the Twist message and sleep 1 cycle self.cmd_vel.publish(move_cmd) r.sleep() # Get the current position (position, rotation) = self.get_odom() # Compute the Euclidean distance from the start distance = sqrt(pow((position.x - x_start), 2) + pow((position.y - y_start), 2)) # Stop the robot before the rotation move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1) # Set the movement command to a rotation move_cmd.angular.z = angular_speed # Track the last angle measured last_angle = rotation # Track how far we have turned turn_angle = 0 while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown(): # Publish the Twist message and sleep 1 cycle self.cmd_vel.publish(move_cmd) r.sleep() # Get the current rotation (position, rotation) = self.get_odom() # Compute the amount of rotation since the last loop delta_angle = normalize_angle(rotation - last_angle) # Add to the running total turn_angle += delta_angle last_angle = rotation # Stop the robot before the next leg move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1) # Stop the robot for good self.cmd_vel.publish(Twist())
def sender(): li = leap_interface.Runner() li.setDaemon(True) li.start() # pub = rospy.Publisher('leapmotion/raw',leap) pub_ros = rospy.Publisher('leapmotion/data',leapros) cmd_pub = rospy.Publisher('cmd_vel_mux/input/teleop', Twist, queue_size=5) rospy.init_node('leap_pub') while not rospy.is_shutdown(): hand_direction_ = li.get_hand_direction() hand_normal_ = li.get_hand_normal() hand_palm_pos_ = li.get_hand_palmpos() hand_pitch_ = li.get_hand_pitch() hand_roll_ = li.get_hand_roll() hand_yaw_ = li.get_hand_yaw() is_hand_ = li.get_is_hand() msg = leapros() msg.direction.x = hand_direction_[0] msg.direction.y = hand_direction_[1] msg.direction.z = hand_direction_[2] msg.normal.x = hand_normal_[0] msg.normal.y = hand_normal_[1] msg.normal.z = hand_normal_[2] msg.palmpos.x = hand_palm_pos_[0] msg.palmpos.y = hand_palm_pos_[1] msg.palmpos.z = hand_palm_pos_[2] msg.ypr.x = hand_yaw_ msg.ypr.y = hand_pitch_ msg.ypr.z = hand_roll_ moveBindings = { 'n':(1,0), 'ne':(1,-1), 'w':(0,1), 'e':(0,-1), 'nw':(1,1), 's':(-1,0), 'se':(-1,1), 'sw':(-1,-1), } #TODO: Adjust movement parameters if is_hand_ == True: print "THERE IS A HAND" if msg.ypr.y < .2 and msg.ypr.y >-.2: #PITCH FOR LINEAR MOVEMENT print "NO LINEAR MOVEMENT" x = 0 elif msg.ypr.y > .2: print "FORWARD" x = 1 elif msg.ypr.y <-.2: print "BACKWARD" x = -1 if msg.ypr.x < .2 and msg.ypr.x >-.2: #ROLL FOR ANGULAR MOVEMENT print "NO ANGULAR MOVEMENT" th = 0 elif msg.ypr.x > .2: print "RIGHT" th = -1 elif msg.ypr.x <-.2: print "LEFT" th = 1 x_move = .1 * x th_move = .1 * th twist = Twist() twist.linear.x = x_move; twist.linear.y = 0; twist.linear.z = 0 twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th_move cmd_pub.publish(twist) else: twist = Twist() twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0 twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0 cmd_pub.publish(twist) # We don't publish native data types, see ROS best practices # pub.publish(hand_direction=hand_direction_,hand_normal = hand_normal_, hand_palm_pos = hand_palm_pos_, hand_pitch = hand_pitch_, hand_roll = hand_roll_, hand_yaw = hand_yaw_) pub_ros.publish(msg) # Save some CPU time, circa 100Hz publishing. rospy.sleep(0.01)
def _get_twist(self, linear, angular): twist = Twist() twist.linear.x = linear twist.angular.z = angular return twist
def tracingWall(): global distance global regions_ msg = Twist() msg.linear.x = 0.5 return msg
def makeVelMsg(self): self.msg_vel = Twist() self.msg_vel.linear.x = self.v self.msg_vel.angular.z = self.w return
def shutdown(self): rospy.loginfo("Stopping TurtleBot") self.pub_cmd_vel_l.publish(Twist()) self.pub_cmd_vel_r.publish(Twist()) rospy.sleep(1)
def avoid_object(msg): global control global incomingVector k=0.1 moveK=5 size=2 error=False objectDetected=False control=False #Send commands to avoid incoming object trajectory #rapid move to avoid objects print "Incoming object!" if(msg.translation.x==0): rx=0 else: rx=1/msg.translation.x*k if(msg.translation.y==0): ry=0 else: ry=1/msg.translation.y*k rz=0 #check map: if slamMap is not None: if(rx<0): xMax = round((robotX-slamMap.info.origin.position.x+size)/slamMap.info.resolution) xMin = round((robotX-slamMap.info.origin.position.x-rx*moveK-size)/slamMap.info.resolution) else: xMax = round((robotX-slamMap.info.origin.position.x+rx*moveK+size)/slamMap.info.resolution) xMin = round((robotX-slamMap.info.origin.position.x-size)/slamMap.info.resolution) if(ry<0): yMax = round((robotY-slamMap.info.origin.position.y+size)/slamMap.info.resolution) yMin = round((robotY-slamMap.info.origin.position.y-rx*moveK-size)/slamMap.info.resolution) else: yMax = round((robotY-slamMap.info.origin.position.y+rx*moveK+size)/slamMap.info.resolution) yMin = round((robotY-slamMap.info.origin.position.y-size)/slamMap.info.resolution) xRob = round((robotX-slamMap.info.origin.position.x)/slamMap.info.resolution) yRob = round((robotY-slamMap.info.origin.position.y)/slamMap.info.resolution) print "Robot coordinates: ", robotX, "-", robotY, "-", robotZ print "Map data: ", slamMap.info.width, "-", slamMap.info.height, "-", slamMap.info.resolution, "-", slamMap.info.origin.position.x, "-", slamMap.info.origin.position.y print "Checking map: ", xMax, "-", xMin, "-", yMax, "-", yMin, " Robot on map: ", int(xRob), "-", int(yRob) for y in range(int(yMin),int(yMax),1): for x in range(int(xMin),int(xMax),1): index = x+y*slamMap.info.width if abs(xRob-x)<1 and abs(yRob-y)<1: if slamMap.data[index] > 90: error = True #sys.stdout.write('R') elif slamMap.data[index] > 90: ## This square is occupied objectDetected = True #sys.stdout.write('X') #elif slamMap.data[index] >= 0: ## This square is unoccupied #sys.stdout.write(" ") #else: #sys.stdout.write('?') #sys.stdout.write('\n') motor_command=Twist() if not error and objectDetected: print "Object detected but can not rapid move" rx=0 ry=0 if robotZ<2: print "at least go up" rz=-k*5 #go up else: print "at least go down" rz=robotZ*3 #go down motor_command.linear.x = -rx motor_command.linear.y = -ry motor_command.linear.z = -rz motor_command_publisher.publish(motor_command) #disable sensor rospy.sleep(0.6) #enable sensor motor_command.linear.x = rx*2 motor_command.linear.y = ry*2 motor_command.linear.z = rz*2 motor_command_publisher.publish(motor_command) control=True incomingVector=msg print(msg)
def follower_node(): rospy.init_node('follower') global robotX global robotY global robotZ global motor_command_publisher motor_command_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size = 10) waypoint_subscriber = rospy.Subscriber("/waypoint_cmd", Transform, waypoint_callback) action_subscriber = rospy.Subscriber("/incoming", Transform, avoid_object) map_subscriber = rospy.Subscriber("/map", OccupancyGrid, map_callback, queue_size = 1000) listener = tf.TransformListener() step=1 size=0.7 dx=0 dy=0 dz=0 print "Starting, dont forget to enable motors" delay = rospy.Rate(50.0); while not rospy.is_shutdown(): motor_command = Twist() objectDetected=False error = False stopped = False if control: try: #grab the latest available transform from the odometry frame (robot's original location - usually the same as the map unless the odometry becomes inaccurate) to the robot's base. (translationZ,orientationZ) = listener.lookupTransform("/base_link", "/base_footprint", rospy.Time(0)); except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: print("EXCEPTION:",e) #if something goes wrong with this just go to bed for a second or so and wake up hopefully refreshed. delay.sleep() continue try: #grab the latest available transform from the odometry frame (robot's original location - usually the same as the map unless the odometry becomes inaccurate) to the robot's base. (translation,orientation) = listener.lookupTransform("/world", "/base_footprint", rospy.Time(0)); except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: print("EXCEPTION:",e) delay.sleep() continue #print "\n---------------------\ntranslation: ", translation, "\norientation: ", orientation, "\nwaypoint:\n", waypoint robotX=translation[0] robotY=translation[1] robotZ=translationZ[2] print "Robot coordinates: ", robotX, "-", robotY, "-", robotZ if slamMap is not None: xMax = round((robotX-slamMap.info.origin.position.x+size)/slamMap.info.resolution) yMax = round((robotY-slamMap.info.origin.position.y+size)/slamMap.info.resolution) xMin = round((robotX-slamMap.info.origin.position.x-size)/slamMap.info.resolution) yMin = round((robotY-slamMap.info.origin.position.y-size)/slamMap.info.resolution) xRob = round((robotX-slamMap.info.origin.position.x)/slamMap.info.resolution) yRob = round((robotY-slamMap.info.origin.position.y)/slamMap.info.resolution) print "Map data: ", slamMap.info.width, "-", slamMap.info.height, "-", slamMap.info.resolution, "-", slamMap.info.origin.position.x, "-", slamMap.info.origin.position.y print "Checking map: ", xMax, "-", xMin, "-", yMax, "-", yMin, " Robot on map: ", int(xRob), "-", int(yRob) #sys.stdout.write('\n') for y in range(int(yMin),int(yMax),1): for x in range(int(xMin),int(xMax),1): index = x+y*slamMap.info.width if abs(xRob-x)<1 and abs(yRob-y)<1: if slamMap.data[index] > 90: error = True #sys.stdout.write('R') elif slamMap.data[index] > 90: ## This square is occupied objectDetected = True #sys.stdout.write('X') #elif slamMap.data[index] >= 0: ## This square is unoccupied #sys.stdout.write(" ") #else: #sys.stdout.write('?') #sys.stdout.write('\n') else: print "Map data not found! Check if slam works" if not error and objectDetected: print("Object detected, stop!") if not stopped and dx!=0 and dy!=0 and dz!=0: motor_command.linear.x = -dx motor_command.linear.y = -dy motor_command.linear.z = -dz #print "command!\n", motor_command.linear.x, "-", motor_command.linear.y, "-", motor_command.linear.z, "-k: ", k motor_command_publisher.publish(motor_command) stopped=True else: if(abs(robotZ)<0.3): #prevent collisions motor_command.linear.z = 0.2 motor_command_publisher.publish(motor_command) rospy.sleep(0.2) dx=waypoint.translation.x-robotX dy=waypoint.translation.y-robotY dz=waypoint.translation.z+robotZ#base link has negative z #print "moving!\n", dx, "-", dy, "-", dz k=1 if(abs(dx)>step or abs(dy)>step or abs(dz)>step): maxd = max(abs(dx),abs(dy),abs(dz)) k = step / maxd dx = dx*k dy = dy*k dz = dz*k motor_command.linear.x = dx motor_command.linear.y = dy motor_command.linear.z = dz #print "command!\n", motor_command.linear.x, "-", motor_command.linear.y, "-", motor_command.linear.z, "-k: ", k motor_command_publisher.publish(motor_command)
def callback_odom(data): global pose_curr, vel_curr pose_curr = data.pose.pose vel_curr = data.twist.twist pub_twist = rospy.Publisher('cmd_vel', Twist, queue_size=10) pub_pose = rospy.Publisher('cmd_pose', Pose, queue_size=10) rospy.init_node('dumb_navigation_planner', anonymous=True) rospy.Subscriber("tick", Int32, callback_tick) rospy.Subscriber('odom', Odometry, callback_odom) # rospy.Subscriber('scan_filtered', LaserScan, callback_scan) rospy.Subscriber('cloud_filtered', PointCloud2, callback_cloud) rate = rospy.Rate(30) val = Twist() pos = Pose() pose_curr = Pose() vel_curr = Twist() fin = True ind = -1 start = False count = 0 chassis_width = rospy.get_param('chassis_width', 1.143) stop_dist = rospy.get_param('stopping_distance', .5) obj_thresh = 10 obj_accum = 0 unwrapper = Unwrapper(math.pi) if rospy.has_param('dumb_plan'):
if target_speed > control_speed: control_speed = min(target_speed, control_speed + 0.02) elif target_speed < control_speed: control_speed = max(target_speed, control_speed - 0.02) else: control_speed = target_speed if target_turn > control_turn: control_turn = min(target_turn, control_turn + 0.1) elif target_turn < control_turn: control_turn = max(target_turn, control_turn - 0.1) else: control_turn = target_turn # 创建并发布twist消息 twist = Twist() twist.linear.x = control_speed twist.linear.y = 0 twist.linear.z = 0 twist.angular.x = 0 twist.angular.y = 0 twist.angular.z = control_turn pub.publish(twist) except BaseException as e: print e finally: twist = Twist() twist.linear.x = 0 twist.linear.y = 0
def turningRight(): msg = Twist() msg.angular.z = -0.25 return msg
front_cone = [] for j in range(355,359): front_cone.append(msg.ranges[j]) for k in range(0,6): front_cone.append(msg.ranges[k]) front = min(front_cone) if __name__ == '__main__': rospy.init_node('gazebo_scan_sub') vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size = 10) scan_sub = rospy.Subscriber('/scan', LaserScan, callback_scan) rate = rospy.Rate(10) vel_msg = Twist() vel_msg.linear.x = 0.1 vel_msg.linear.y = 0 vel_msg.linear.z = 0 vel_msg.angular.x = 0 vel_msg.angular.y = 0 vel_msg.angular.z = 0 while not rospy.is_shutdown(): while front < 0.75: if left_dist == float('inf'): dir = 1 elif right_dist == float('inf'): dir = -1 elif left_dist>right_dist: dir = 1 else: dir = -1 vel_msg.angular.z = dir * 0.75
def turningLeft(): msg = Twist() msg.angular.z = 0.5 return msg
def newOdom(msg): global x, y, second_x, second_y, temp1_x, temp2_x, temp1_y, temp2_y global theta global pointList global listnum, listlength global init_y, init_x global flag global r2d global yaw, yaw_temp global robot_path global two_robot_point global time_flag, data_flag global start_time global size_x, arrive_flag, err_dist_list, goal_angle_list, theta_list global goal2_theta, push_dist, push_flag, other_dist two_robot_point = [ float("{0:.3f}".format(size_x - x)), float("{0:.3f}".format(y)) ] robot_path.append(two_robot_point) speed = Twist() if flag == 0 and data_flag == 1: init_x = msg.pose.pose.position.x * r_size_x init_y = msg.pose.pose.position.y * r_size_y temp1_x = init_x temp2_x = init_x temp1_y = init_y temp2_y = init_y flag = 1 elif flag == 1 or flag == 2 or flag == 3: if time_flag == 0: start_time = rospy.Time.now() print("start time : ", start_time) time_flag = 1 x = msg.pose.pose.position.x * r_size_x + size_x - pointList[0] y = msg.pose.pose.position.y * r_size_y + pointList[1] temp2_x = temp1_x temp1_x = x x = (temp1_x + temp2_x) / 2.0 temp2_y = temp1_y temp1_y = y y = (temp1_y + temp2_y) / 2.0 rot_q = msg.pose.pose.orientation (roll, pitch, theta) = euler_from_quaternion([rot_q.x, rot_q.y, rot_q.z, rot_q.w]) if (theta > pi): theta = theta - 2.0 * pi elif (theta < -pi): theta = theta + 2.0 * pi if flag == 1 or flag == 2: start() if listnum == listlength and flag == 2: err_theta = get_last_err_theta(goal2_theta / r2d) err_theta_d = err_theta * r2d pid_theta = thetaPID(err_theta) if ((err_theta_d > 3 and err_theta_d < 360) or (err_theta_d < -3 and err_theta_d > -360)): speed.linear.x = 0.0 if (err_theta_d < 0): speed.angular.z = pid_theta else: speed.angular.z = -pid_theta else: speed.angular.z = 0.0 finish_time = rospy.Time.now() print('finish time : ', finish_time) print((finish_time - start_time) / 1000000000.0) second_x = x second_y = y flag = 3 pub.publish(speed) elif flag == 3 and push_flag == 1 and arrive_flag == 1: other_dist = forword_dist(push_dist) up_down = direction() if up_down == "foward": if other_dist <= 1.2: speed.angular.z = 0.0 speed.linear.x = 0.0 flag = 4 else: speed.angular.z = 0.0 speed.linear.x = other_dist * 0.01 else: if other_dist <= 1.2: speed.angular.z = 0.0 speed.linear.x = 0.0 flag = 4 else: speed.angular.z = 0.0 speed.linear.x = -1 * other_dist * 0.01 pub.publish(speed) elif flag == 4: file = open('/home/dodo/robot2_path.txt', 'w') for a in range(0, len(robot_path)): line = "%5.2f %5.2f \n" % (robot_path[a][0], robot_path[a][1]) file.write(line) file.close() f = open('/home/dodo/robot2_path.txt', 'r') lines = f.readlines() plot_path(pointList, robot_path) two_robot_point = [] err_dist_list = [] goal_angle_list = [] theta_list = [] pointList = 0 data_flag = 0 listnum = 0 flag = 0 arrive_flag = 0 push_flag = 0 arrive.publish(flag)
def publisher_node(self): rospy.sleep(2) twist = Twist() init_t = rospy.get_time() rate = rospy.Rate(10) #Line-following Controller Initialization desired = 300 # Line index value at which robot is centred k_p = 0.0057 correction = 0 #Bayesian Localization Parameter Initialization state_model = [0.05, 0.1, 0.85] # State model for u_k = +1 meas_model = { 0: { 0: 0.6, 1: 0.05, 2: 0.2, 3: 0.05 }, #green 1: { 0: 0.05, 1: 0.6, 2: 0.05, 3: 0.15 }, #orange 2: { 0: 0.2, 1: 0.05, 2: 0.6, 3: 0.05 }, #blue 3: { 0: 0.05, 1: 0.2, 2: 0.05, 3: 0.65 }, #yellow 4: { 0: 0.1, 1: 0.1, 2: 0.1, 3: 0.1 } } #line #Measurement_model represented as dictionary with keys as measurement z_k and value being a dictionary with key x_k and corresponding probabilities pred = np.zeros(12) # Position prediction at each office location update = np.zeros( 12, dtype=np.float ) # Update to state estimation probabilities based on prediction and measurement model norm = np.zeros( 12 ) # Normalization constant at each update to the state estimation curr_state = [1 / float(len(pred))] * len( pred ) # Current state estimation, starting with uniform probability distribution # Reference RGB colour codes ref = np.zeros((5, 3)) ref[0] = [133, 163, 1] #green ref[1] = [222, 48, 1] #orange ref[2] = [35, 60, 82] #blue ref[3] = [171, 140, 2] #yellow ref[4] = [120, 96, 34] #line j = 0 while rospy.get_time() - init_t < (250): actual = int(self.line_idx) colour = None rgb_error = np.abs(ref - self.measured_rgb) rgb_sum = np.sum(rgb_error, axis=1) min_ind = np.argmin(rgb_sum) # Colour estimation if ((min_ind == 0) or (min_ind == 1) or (min_ind == 2) or (min_ind == 3)): # If a colour is seen correction = 0 colour = min_ind elif (min_ind == 4 ): # If no colour measured/seen (detected a line) error = desired - actual correction = k_p * error # Correction for P control twist.angular.z = correction twist.linear.x = 0.05 self.cmd_pub.publish(twist) curr_office = np.argmax(curr_state) + 1 # Current state prediction print(curr_state, (curr_office)) if correction == 0 and colour != None: # If a colour is seen if curr_office == 4 or curr_office == 6 or curr_office == 8: # If we are at our chosen office location for delivery init_2 = rospy.get_time() rospy.sleep(3) while rospy.get_time() - init_2 < ( (math.pi) / .32): #90 degree left rotation twist.linear.x = 0 twist.angular.z = 0.2 self.cmd_pub.publish(twist) rate.sleep() init_3 = rospy.get_time() rospy.sleep(1) # Pause while rospy.get_time() - init_3 < ( (math.pi) / .48): #90 degree right rotation twist.linear.x = 0 twist.angular.z = -0.2 self.cmd_pub.publish(twist) rate.sleep() # Resume straight-line motion twist.linear.x = 0.05 twist.angular.z = 0 self.cmd_pub.publish(twist) rospy.sleep(3) else: rospy.sleep(6.69) # Continue straight-line motion for i in range(0, len(curr_state)): # Position prediction step if (i == 0): # First point in map step_fwd = curr_state[len(curr_state) - 1] * state_model[2] if (i != 0): step_fwd = curr_state[i - 1] * state_model[2] #loop around if (i == len(curr_state) - 1): step_bwd = curr_state[0] * state_model[0] if (i != len(curr_state) - 1): step_bwd = curr_state[i + 1] * state_model[0] step_none = curr_state[i] * state_model[1] pred[i] = (step_fwd + step_bwd + step_none) for k in range(0, len(pred)): #update based on measurement update[k] = pred[k] * meas_model[colour][color_map[k]] norm[j] += update[k] update = [float(x) / float(norm[j]) for x in update] curr_state = update # This evolves at each colour measurement j += 1 # Increment j every time you get a measurement rate.sleep() pass
# recebedor = rospy.Subscriber(topico_imagem, CompressedImage, roda_todo_frame, queue_size=4, buff_size=2**24) print("Usando ", topico_imagem) velocidade_saida = rospy.Publisher("/cmd_vel", Twist, queue_size=1) recebe_scan = rospy.Subscriber("/scan", LaserScan, scaneou) try: while not rospy.is_shutdown(): vel = Twist(Vector3(0, 0, 0), Vector3(0, 0, 0)) if dist <= 0.25: vel = Twist(Vector3(0, 0, 0), Vector3(0, 0, 0)) forward = Twist(Vector3(0, 0, 0), Vector3(0, 0, 0)) t = 0.1 else: print(dist) if true == 0: forward = Twist(Vector3(0, 0, 0), Vector3(0, 0, 0)) t = 0 elif true != 0: forward = Twist(Vector3(0.1, 0, 0), Vector3(0, 0, 0))
def control_callback(event): # Actual control is done here. The 'event' is the rospy.Timer() duration period, can be used # for trubleshooting. To test how often is executed use: $ rostopic hz /mallard/thruster_commands. global x0,y0,q0,x_goal,y_goal,q_goal,ed global time_elapsed,t_goal,t_goal_psi global t_ramp,psivel twist = Twist() # Get forces in global frame using PD controller if(goals_received == True and joy_button_L1 == 0 and joy_button_R1 == 0): # new code - velocity ramp # get current time tn = rospy.Time.now() time_now = tn.secs + (tn.nsecs * 0.000000001) # time elapsed form the start of tracking the goal (equal to t_now in goal selector) time_elapsed = time_now - time_begin # get max velocities xvelmax = abs(kguseful.safe_div((x_goal-x0),t_goal)) yvelmax = abs(kguseful.safe_div((y_goal-y0),t_goal)) # get desired position,velocity and acceleration from velocity ramp x_des, x_vel_des, x_acc_des = kglocal.velramp(time_elapsed, xvelmax, x0, x_goal, t_ramp,name="x") y_des, y_vel_des, y_acc_des = kglocal.velramp(time_elapsed, yvelmax, y0, y_goal, t_ramp,name="y") # get desired angle: qdes = kglocal.despsi_fun(q_goal, t_goal_psi, q0, time_elapsed) psi_des = tft.euler_from_quaternion(qdes)[2] # Its a list: (roll,pitch,yaw) psi_vel_des = kglocal.desvelpsi_fun(ed, t_goal_psi, time_elapsed,psivel) # end new code # PD global control x_global_ctrl = control.proportional(x, x_goal, x_vel, x_vel_goal, param['kp'], param['kd'], param['lim']) y_global_ctrl = control.proportional(y, y_goal, y_vel, y_vel_goal, param['kp'], param['kd'], param['lim']) psi_global_ctrl = control.proportional_angle(psi, psi_goal,psi_vel,psi_vel_goal, param['kp_psi'], param['kd_psi'], param['lim_psi']) # Convert to body coordiante x_PD_body = math.cos(psi)*x_global_ctrl + math.sin(psi)*y_global_ctrl y_PD_body =-math.sin(psi)*x_global_ctrl + math.cos(psi)*y_global_ctrl # Publish to twist velocitycommand twist.linear.x = x_PD_body twist.linear.y = y_PD_body twist.angular.z = -psi_global_ctrl # Generate time instance now = rospy.Time.now() twist.angular.x = now.secs twist.angular.y = now.nsecs # Publish forces to simulation (joint_state_publisher message) pub_velocity.publish(twist) # send [time,position,velocity,goal_position,goal_velocity,control input] array_data = [now.secs,now.nsecs,\ x,y,psi,x_vel,y_vel,psi_vel,\ x_des,y_des,psi_des,x_vel_des,y_vel_des,psi_vel_des,\ x_body_model_ctrl,y_body_model_ctrl,psi_global_ctrl] data_to_send = Float64MultiArray(data = array_data) pub_data.publish(data_to_send) elif(joy_button_L1 == 1 or joy_button_R1 == 1): twist.linear.x = joy_x twist.linear.y = joy_y twist.angular.z = joy_z pub_velocity.publish(twist) else: # ----- idle if no goals ----- pub_velocity.publish(Twist())
def shutdown(self): # Always stop the robot when shutting down the node. rospy.loginfo("Stopping the robot...") self.cmd_vel.publish(Twist()) rospy.sleep(1)
def __init__(self): #give the node a name rospy.init_node('calibrate_linear', anonymous=False) #set rospy to execute a shutdown function when terminating the script rospy.on_shutdown(self.shutdown) #How fast will we check the odometry values? self.rate = 10 r = rospy.Rate(self.rate) #set the distance to travel self.test_distance = 1.5 self.speed = 0.2 self.tolerance = 0.01 self.odom_linear_scale_correction = 1.0 self.start_test = True #Publisher to control the robot's speed self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) #The base frame is base_footprint for the robot self.base_frame = rospy.get_param('~base_frame', '/base_footprint') #The odom frame is usually just /odom self.odom_frame = rospy.get_param('~odom_frame', '/odom') #initialize the tf listener self.tf_listener = tf.TransformListener() #give tf some time to fill its buffer rospy.sleep(2) #make sure we see the odom and base frames self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0)) self.position = Point() #get the starting position from the tf transform between the odom and base frames self.position = self.get_position() x_start = self.position.x y_start = self.position.y move_cmd = Twist() while not rospy.is_shutdown(): #Stop the robot by default move_cmd = Twist() if self.start_test: #get the current position from the tf transform between the odom and base frames self.position = self.get_position() #compute the euclidean distance from the target point distance = sqrt( pow((self.position.x - x_start), 2) + pow((self.position.y - y_start), 2)) #correct the estimate distance by the correction factor distance *= self.odom_linear_scale_correction #How close are we? error = distance - self.test_distance #are we close enough? if not self.start_test or abs(error) < self.tolerance: self.start_test = False params = False rospy.loginfo(params) else: #if not, move in the appropriate direction move_cmd.linear.x = copysign(self.speed, -1 * error) else: self.position = self.get_position() x_start = self.position.x y_start = self.position.y self.cmd_vel.publish(move_cmd) r.sleep() #stop the robot self.cmd_vel.publish(Twist())
def TwistKDLToMsg(kdl_twist): msg_twist = Twist() msg_twist.linear = Vector3(*kdl_twist.vel) msg_twist.angular = Vector3(*kdl_twist.rot) return msg_twist
def shutdown(self): twist = Twist() twist.linear.x = 0 twist.angular.z = 0 self.pub_vel.publish(twist) print("shutdown")
import roslib import rospy roslib.load_manifest('drone') # Import the messages we're interested in sending and receiving from geometry_msgs.msg import Twist # for sending commands to the drone from std_msgs.msg import Float64 # for the control_effort # Allow the controller to publish to the /cmd_vel topic and thus control the drone #global variable so we can use it inside the callback pubCmdTo_drone = rospy.Publisher('drone2/cmd_vel', Twist, queue_size=1) #global variable because I dont want to reset to zero the previous command in every callback. Otherwise the drone would be stopped position command = Twist() def ApplyControlEffort_Yaw(controlEffort): global command yaw_velocity=controlEffort.data command.angular.z = yaw_velocity pubCmdTo_drone.publish(command) PrintCommands() def ApplyControlEffort_Pitch(controlEffort): global command pitch = controlEffort.data #need the minus pitch so it goes the right way command.linear.x = pitch pubCmdTo_drone.publish(command)
def execute_cb(self, goal): rate = rospy.Rate(20) success = True # Publish move_base goal marker quaternion = goal.target_pose.pose.orientation marker = Marker() marker.header.frame_id = "map" marker.header.stamp = rospy.Time.now() marker.id = 0 marker.type = 0 marker.action = Marker.ADD marker.pose.position.x = goal.target_pose.pose.position.x marker.pose.position.y = goal.target_pose.pose.position.y marker.pose.position.z = 0 marker.pose.orientation = quaternion marker.scale.x = 1.0 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.b = 1.0 marker.color.g = 0.0 marker.frame_locked = True self.goalmarkerPublisher.publish(marker) pos = Pose2D(self.x, self.y, self.theta) # Check if PR2 is inside the bounds if not self.check_bounds(pos, self.HardBound): rospy.logwarn( 'Trixi is not in the legal area. Please use the joystick to navigate into the legal area.' ) #self._result.success = False #self._result.error_message = "Trixi is not in the legal area. Please use the joystick to navigate into the legal area." self._as.set_aborted(self._result) return # Check if the target is inside the bounds #self._transformer.waitForTransform('map', goal.target_pose.header.frame_id, rospy.Time(), rospy.Duration(2)) goalMap = self._tfBuffer.transform(goal.target_pose, "map") ignored1, ignored2, yaw = euler_from_quaternion([ goalMap.pose.orientation.x, goalMap.pose.orientation.y, goalMap.pose.orientation.z, goalMap.pose.orientation.w ]) goal2D = Pose2D(goalMap.pose.position.x, goalMap.pose.position.y, yaw) if not self.check_bounds(goal2D, self.SoftBound): rospy.logwarn('Requested point is outside the legal soft bound.') #self._result.success = False #self._result.error_message = "Requested point is outside the legal bounds." self._as.set_aborted(self._result) return integral_angle = 0 integral_translation_x = 0 integral_translation_y = 0 while not rospy.is_shutdown(): # Check if PR2 is inside the bounds pos.updatePose(self.x, self.y, self.theta) if not self.check_bounds(pos, self.HardBound): rospy.logwarn( 'Trixi is not in the legal area. Please use the joystick to navigate into the legal area.' ) #self._result.success = False #self._result.error_message = "Trixi is not in the legal area. Please use the joystick to navigate into the legal area." self._as.set_aborted(self._result) return if self._as.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self._action_name) self._as.set_preempted() success = False break try: goal.target_pose.header.stamp = rospy.Time( ) #rospy.get_rostime() goalBaseLink = self._tfBuffer.transform( goal.target_pose, 'base_footprint') dir = goalBaseLink.pose.position ignored1, ignored2, angle_diff = euler_from_quaternion([ goalBaseLink.pose.orientation.x, goalBaseLink.pose.orientation.y, goalBaseLink.pose.orientation.z, goalBaseLink.pose.orientation.w ]) # TODO: fine tune on PR2 # TODO: PI-controller translation_error = math.sqrt(dir.x**2 + dir.y**2) integral_angle += angle_diff integral_translation_x += dir.x integral_translation_y += dir.y if not (-0.087 < angle_diff < 0.087): # ca. 5 degrees of tollerance message = Twist() max_ang_vel = math.pi / 6 # TODO fine tune on PR2 message.angular.z = clamp(angle_diff * 0.8, -max_ang_vel, max_ang_vel) self.publisher.publish(message) elif not (translation_error < 0.1): message = Twist() max_lin_vel = 0.15 # TODO fine tune on PR2 message.linear.x = clamp(dir.x, -max_lin_vel, max_lin_vel) message.linear.y = clamp(dir.y, -max_lin_vel, max_lin_vel) self.publisher.publish(message) else: break # TODO: fill out move_base feedback # should you use the amcl position or just ask tf for (0,0,0) base_footprint transformed to map self._as.publish_feedback(self._feedback) # TODO: doublecheck the documentation if these exceptions are still relevant in tf2_ros for the transform method except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: print("failed to get transform") print("[" + str(type(e)) + "]" + str(e)) rate.sleep() continue rate.sleep() if success: #self._result.success = True #self._result.error_message = "We successfully reached the specified goal." rospy.loginfo('Succeeded') self._as.set_succeeded(self._result)
class Controller(object): tele_twist = Twist() def __init__(self, mode, scale_x, scale_z, rate): self._mode = mode self._scale_x = scale_x self._scale_z = scale_z self._timer = Timer() self._rate = rospy.Rate(rate) self._enable_auto_control = False self.current_control = None self.trajectory_index = None self.bridge = CvBridge() # callback data store self.image = None self.left_img = None self.right_img = None self.depth_img = None self.me1_left = None self.me1_right = None self.me1_depth = None self.me2_left = None self.me2_right = None self.me2_depth = None self.me3_left = None self.me3_right = None self.me3_depth = None self.intention = None self.imu = None self.imu1 = None self.imu2 = None self.imu3 = None self.odom = None self.speed = None self.labeled_control = None self.key = None self.training = False self.scan = None self.manual_intention = 'forward' self.is_manual_intention = False # subscribe ros messages rospy.Subscriber('/mynteye/left/image_raw/compressed', CompressedImage, self.cb_left_img, queue_size=1, buff_size=2**10) rospy.Subscriber('/mynteye/right/image_raw/compressed', CompressedImage, self.cb_right_img, queue_size=1, buff_size=2**10) rospy.Subscriber('/mynteye/depth/image_raw/compressed', CompressedImage, self.cb_depth_img, queue_size=1, buff_size=2**10) rospy.Subscriber('/mynteye_1/left/image_raw/compressed', CompressedImage, self.cb_me1_left_img, queue_size=1, buff_size=2**10) rospy.Subscriber('/mynteye_1/right/image_raw/compressed', CompressedImage, self.cb_me1_right_img, queue_size=1, buff_size=2**10) rospy.Subscriber('/mynteye_1/depth/image_raw/compressed', CompressedImage, self.cb_me1_depth_img, queue_size=1, buff_size=2**10) rospy.Subscriber('/mynteye_2/left/image_raw/compressed', CompressedImage, self.cb_me2_left_img, queue_size=1, buff_size=2**10) rospy.Subscriber('/mynteye_2/right/image_raw/compressed', CompressedImage, self.cb_me2_right_img, queue_size=1, buff_size=2**10) rospy.Subscriber('/mynteye_2/depth/image_raw/compressed', CompressedImage, self.cb_me2_depth_img, queue_size=1, buff_size=2**10) rospy.Subscriber('/mynteye_3/left/image_raw/compressed', CompressedImage, self.cb_me3_left_img, queue_size=1, buff_size=2**10) rospy.Subscriber('/mynteye_3/right/image_raw/compressed', CompressedImage, self.cb_me3_right_img, queue_size=1, buff_size=2**10) rospy.Subscriber('/mynteye_3/depth/image_raw/compressed', CompressedImage, self.cb_me3_depth_img, queue_size=1, buff_size=2**10) rospy.Subscriber('/scan', LaserScan, self.cb_scan, queue_size=1, buff_size=2**10) rospy.Subscriber('/imu', Imu, self.cb_imu, queue_size=1, buff_size=2**10) rospy.Subscriber('/imu1', Imu, self.cb_imu1, queue_size=1, buff_size=2**10) rospy.Subscriber('/imu2', Imu, self.cb_imu2, queue_size=1, buff_size=2**10) rospy.Subscriber('/imu3', Imu, self.cb_imu3, queue_size=1, buff_size=2**10) if mode == 'DLM': rospy.Subscriber('/test_intention', String, self.cb_dlm_intention, queue_size=1) else: rospy.Subscriber('/intention_lpe', Image, self.cb_lpe_intention, queue_size=1, buff_size=2**10) rospy.Subscriber('/speed', Float32, self.cb_speed, queue_size=1) rospy.Subscriber('/odometry/filtered', Odometry, self.cb_odom, queue_size=1, buff_size=2**10) rospy.Subscriber('/joy', Joy, self.cb_joy) # publish control self.control_pub = rospy.Publisher('/RosAria/cmd_vel', Twist, queue_size=1) # publishing as training data self.pub_intention = rospy.Publisher('/test_intention', String, queue_size=1) self.pub_trajectory_index = rospy.Publisher('/train/trajectory_index', String, queue_size=1) self.pub_teleop_vel = rospy.Publisher('/train/cmd_vel', Twist, queue_size=1) self.pub_left_img = rospy.Publisher( '/train/mynteye/left_img/compressed', CompressedImage, queue_size=1) self.pub_right_img = rospy.Publisher( '/train/mynteye/right_img/compressed', CompressedImage, queue_size=1) self.pub_depth_img = rospy.Publisher( '/train/mynteye/depth_img/compressed', CompressedImage, queue_size=1) self.pub_me1_left_img = rospy.Publisher( '/train/mynteye_1/left_img/compressed', CompressedImage, queue_size=1) self.pub_me1_right_img = rospy.Publisher( '/train/mynteye_1/right_img/compressed', CompressedImage, queue_size=1) self.pub_me1_depth_img = rospy.Publisher( '/train/mynteye_1/depth_img/compressed', CompressedImage, queue_size=1) self.pub_me2_left_img = rospy.Publisher( '/train/mynteye_2/left_img/compressed', CompressedImage, queue_size=1) self.pub_me2_right_img = rospy.Publisher( '/train/mynteye_2/right_img/compressed', CompressedImage, queue_size=1) self.pub_me2_depth_img = rospy.Publisher( '/train/mynteye_2/depth_img/compressed', CompressedImage, queue_size=1) self.pub_me3_left_img = rospy.Publisher( '/train/mynteye_3/left_img/compressed', CompressedImage, queue_size=1) self.pub_me3_right_img = rospy.Publisher( '/train/mynteye_3/right_img/compressed', CompressedImage, queue_size=1) self.pub_me3_depth_img = rospy.Publisher( '/train/mynteye_3/depth_img/compressed', CompressedImage, queue_size=1) self.pub_intention = rospy.Publisher('/train/intention', String, queue_size=1) self.pub_imu = rospy.Publisher('/train/imu', Imu, queue_size=1) self.pub_imu1 = rospy.Publisher('/train/imu1', Imu, queue_size=1) self.pub_imu2 = rospy.Publisher('/train/imu2', Imu, queue_size=1) self.pub_imu3 = rospy.Publisher('/train/imu3', Imu, queue_size=1) self.pub_odom = rospy.Publisher('/train/odometry/filtered', Odometry, queue_size=1) self.pub_scan = rospy.Publisher('/train/scan', LaserScan, queue_size=1) def cb_scan(self, msg): self.scan = msg def cb_left_img(self, msg): self.left_img = msg def cb_right_img(self, msg): self.right_img = msg def cb_depth_img(self, msg): self.depth_img = msg def cb_me1_left_img(self, msg): self.me1_left = msg def cb_me1_right_img(self, msg): self.me1_right = msg def cb_me1_depth_img(self, msg): self.me1_depth = msg def cb_me2_left_img(self, msg): self.me2_left = msg def cb_me2_right_img(self, msg): self.me2_right = msg def cb_me2_depth_img(self, msg): self.me2_depth = msg def cb_me3_left_img(self, msg): self.me3_left = msg def cb_me3_right_img(self, msg): self.me3_right = msg def cb_me3_depth_img(self, msg): self.me3_depth = msg def cb_dlm_intention(self, msg): self.intention = msg.data def cb_lpe_intention(self, msg): self.intention = cv2.resize( undistort(CvBridge().imgmsg_to_cv2(msg, desired_encoding='bgr8')), (224, 224)) def cb_speed(self, msg): self.speed = msg.data def cb_labeled_control(self, msg): self.labeled_control = msg def cb_imu(self, msg): self.imu = msg def cb_imu1(self, msg): self.imu1 = msg def cb_imu2(self, msg): self.imu2 = msg def cb_imu3(self, msg): self.imu3 = msg def cb_odom(self, msg): self.odom = msg def cb_joy(self, data): self.tele_twist.linear.x = self._scale_x * data.axes[ JOY_MAPPING['axes']['left_stick_ud']] self.tele_twist.angular.z = self._scale_z * data.axes[ JOY_MAPPING['axes']['left_stick_lr']] # parse control key if data.buttons[JOY_MAPPING['buttons']['A']] == 1: self._enable_auto_control = True print('Auto control') if data.buttons[JOY_MAPPING['buttons']['B']] == 1: self._enable_auto_control = False print('Manual control') if data.buttons[JOY_MAPPING['buttons']['back']] == 1: self.key = 'q' # toggle recording mode and generate trajectory index if start recording if data.buttons[JOY_MAPPING['buttons']['start']] == 1: self.key = 't' print('toggle training mode to: %s' % (not self.training)) if data.buttons[JOY_MAPPING['buttons']['mode']] == 1: self.key = 'i' print('toggle intention mode to: %s' % (not self.is_manual_intention)) #STRAIGHT_FORWARD if data.buttons[JOY_MAPPING['buttons']['X']] == 1: self.manual_intention = 'forward' print('Intention is manually set to: forward') #LEFT_TURN if data.buttons[JOY_MAPPING['buttons']['lt']] == 1: self.manual_intention = 'left' print('Intention is manually set to: left') #RIGHT_TURN if data.buttons[JOY_MAPPING['buttons']['rt']] == 1: self.manual_intention = 'right' print('Intention is manually set to: right') def _random_string(self, n): chars = string.ascii_letters + string.digits ret = ''.join(random.choice(chars) for _ in range(n)) return ret def _on_loop(self, policy): """ Logical loop """ self._timer.tick() if self.key == 'q': #sys.exit(-1) self.intention = 'stop' self.key = '' if self.key == 't': self.training = not self.training self.key = '' if self.key == 'i': self.is_manual_intention = not self.is_manual_intention self.key = '' if self._enable_auto_control: if self.is_manual_intention: intention = Dataset.INTENTION_MAPPING[self.manual_intention] else: intention = Dataset.INTENTION_MAPPING[ self.intention] # map intention str => int if not intention: print('estimate pose + goal....') elif intention == 'stop': self.tele_twist.linear.x = 0 self.tele_twist.angular.z = 0 else: print('intention: ', intention) # convert ros msg -> cv2 inp = self.get_data(intention) pred_control = np.array(policy(*inp))[0] self.tele_twist.linear.x = pred_control[ 0] * Dataset.SCALE_VEL * 0.8 self.tele_twist.angular.z = pred_control[ 1] * Dataset.SCALE_STEER * 0.8 # publish to /train/* topic to record data (if in training mode) if self.training: self._publish_as_trn() # publish control self.control_pub.publish(self.tele_twist) def get_data(self, intention): mrgb = cv2.resize( undistort( self.bridge.compressed_imgmsg_to_cv2(self.right_img, desired_encoding='bgr8'), FRONT_CAMERA_INFO), (3, 224, 224)) mbnw = rgb2gray(mrgb) rbnw = cv2.resize( undistort( self.bridge.compressed_imgmsg_to_cv2(self.me2_left, desired_encoding='bgr8'), RIGHT_CAMERA_INFO), (224, 224)) lbnw = cv2.resize( undistort( self.bridge.compressed_imgmsg_to_cv2(self.me3_right, desired_encoding='bgr8'), LEFT_CAMERA_INFO), (224, 224)) dm = cv2.resize( self.bridge.compressed_imgmsg_to_cv2(self.depth_img, desired_encoding='bgr8'), (224, 224)) dr = cv2.resize( self.bridge.compressed_imgmsg_to_cv2(self.me2_depth, desired_encoding='bgr8'), (224, 224)) dl = cv2.resize( self.bridge.compressed_imgmsg_to_cv2(self.me3_depth, desired_encoding='bgr8'), (224, 224)) #preprocess mbnw = torch.tensor(mbnw).expand(1, 1, 224, 224).float() / 255.0 rbnw = torch.tensor(rbnw).expand(1, 1, 224, 224).float() / 255.0 lbnw = torch.tensor(lbnw).expand(1, 1, 224, 224).float() / 255.0 dm = torch.tensor(dm).expand(1, 1, 224, 224).float() / 255.0 dr = torch.tensor(dr).expand(1, 1, 224, 224).float() / 255.0 dl = torch.tensor(dl).expand(1, 1, 224, 224).float() / 255.0 intention = torch.tensor([intention]).long() return [intention, dl, dm, dr, lbnw, mbnw, rbnw] def _publish_as_trn(self): if self.odom: self.pub_trajectory_index.publish(self.trajectory_index) self.pub_left_img.publish(self.left_img) self.pub_right_img.publish(self.right_img) self.pub_depth_img.publish(self.depth_img) self.pub_me1_left_img.publish(self.me1_left) self.pub_me1_right_img.publish(self.me1_right) self.pub_me1_depth_img.publish(self.me1_depth) self.pub_me2_left_img.publish(self.me2_left) self.pub_me2_right_img.publish(self.me2_right) self.pub_me2_depth_img.publish(self.me2_depth) self.pub_me3_left_img.publish(self.me3_left) self.pub_me3_right_img.publish(self.me3_right) self.pub_me3_depth_img.publish(self.me3_depth) self.pub_teleop_vel.publish(self.tele_twist) self.pub_intention.publish(self.intention) self.pub_imu.publish(self.imu) self.pub_imu1.publish(self.imu1) self.pub_imu2.publish(self.imu2) self.pub_imu3.publish(self.imu3) self.pub_odom.publish(self.odom) self.pub_scan.publish(self.scan) def execute(self, policy): while True: self._on_loop(policy) self._rate.sleep()
#!/usr/bin/env python import rospy from geometry_msgs.msg import Twist from sensor_msgs.msg import Joy from std_msgs.msg import String g_is_dead_bot = True g_red_light_on = False g_twist = Twist() # default to 0 def dead_bot_switch(msg): global g_is_dead_bot if msg.buttons[2] == 1: g_is_dead_bot = (g_is_dead_bot == False) # print(g_is_dead_bot) def got_twist(msg): global g_twist g_twist = msg def safety_switch(msg): global g_red_light_on # print(msg.data=="Go") if msg.data == "Go":