def reports_callback(self, msg): nowTime = rospy.Time.now() try: self.prev_state = self.state except AttributeError as ae: self.prev_state = msg.state self.state = msg.state self.robot_x = msg.state.position_x self.robot_y = msg.state.position_y self.robot_h = msg.state.orientation_angle self.robot_v = 0 self.robot_w = 0 inc_t = (nowTime - self.prev_time).to_sec() inc_x = self.state.position_x - self.prev_state.position_x inc_y = self.state.position_y - self.prev_state.position_y inc_r = np.sqrt((inc_x**2) + (inc_y**2)) self.robot_v = inc_r / inc_t inc_h = self.state.orientation_angle - self.prev_state.orientation_angle self.robot_w = inc_h / inc_t rospy.logdebug_throttle( 5, "Node [" + rospy.get_name() + "] " + "Robot status: Pose ( " + str(self.state.position_x) + ", " + str(self.state.position_y) + ", " + str(self.state.orientation_angle * 180.0 / np.pi) + " deg), " + "Speed ( " + str(self.robot_v) + " m/sec, " + str(self.robot_w * 180.0 / np.pi) + " deg/sec) ") self.prev_time = nowTime
def closest_waypoint_ahead(self, pos_x, pos_y, yaw, waypoints): ''' Return index of closest point ahead ''' # Create some logging info loginfo = 'yaw: {} | x: {} | y: {}'.format(yaw, pos_x, pos_y) # Define unit vector for car orientation in global (x, y) coordinates orient_x, orient_y = math.cos(yaw), math.sin(yaw) # Filter waypoints to keep only the ones ahead of us by checking scalar product waypoints_ahead = [ (n, wp) for (n, wp) in enumerate(waypoints) if (orient_x * (wp.pose.pose.position.x - pos_x) + orient_y * (wp.pose.pose.position.y - pos_y)) > 0 ] if not len(waypoints_ahead): rospy.logwarn("No points detected ahead of us") # Extract closest waypoint closest_waypoint = min( waypoints_ahead, key=lambda wpidx: (wpidx[1].pose.pose.position.x - self.ego_pos.position.x)**2 + (wpidx[1].pose.pose.position.y - self.ego_pos.position.y)**2) closest_index = closest_waypoint[0] loginfo += '| Closest waypoint index: {}'.format(closest_index) rospy.logdebug_throttle(0.1, loginfo) return closest_index
def check_obstacle(self): obstacle = ['no_obstacle', 0, 0] try: urgent = [(1 if x < u else 0) for x, u in zip(self.ir_array_filtered, URGENT_BD)] if (1 in urgent): obstacle[0] = 'urgent' if (1 in urgent[0:NUM_SS_FRONT]): rospy.loginfo("Urgent front! Front: {}".format( self._list(self.ir_front))) obstacle[1] = 1 if (1 in urgent[NUM_SS_FRONT:NUM_SENSOR]): rospy.loginfo("Urgent back! Back: {}".format( self._list(self.ir_back))) obstacle[2] = 1 return obstacle else: rospy.logdebug_throttle(1, self.bubble_boundary[0:NUM_SS_FRONT]) # Xac dinh vi tri co vat can self.obstacle_located = [(1 if x < bb else 0) for x,bb in \ zip(self.ir_front, \ self.bubble_boundary[0:NUM_SS_FRONT])] if (1 in self.obstacle_located): rospy.logdebug("obstacle_locate: {}".format( self.obstacle_located)) obstacle[0] = 'bubble_obstacle' obstacle[1] = 1 # return obstacle except Exception as e: rospy.logerr(e) finally: return obstacle
def read_encoder_data(self): try: struct_len = ord(self.serial_conn.read()) data_str = self.serial_conn.read(struct_len) rec_checksum = ord(self.serial_conn.read()) except serial.SerialTimeoutException: rospy.logerr("Serial timeout awaiting encoder data") return False # Use the Python struct library to interpret the data # '<' enforces little-endianness # L[n]l means: # L - one unsigned long (timestamp) # [n]l - [n] signed longs (encoder counts) try: format_str = "<L{}l".format(NUM_ENCODERS) encoder_data = struct.unpack(format_str, data_str) except struct.error: rospy.logerr("read_encoder_data error in struct unpack. Message: %s", data_str) return False if rec_checksum == calc_checksum(data_str): self.encoder_callback(encoder_data) rospy.logdebug_throttle(2, "encoder message received") else: rospy.logerr("Encoder checksum failure, ignoring message")
def control(self, cur_linear, cur_ang, target_linear, target_ang, dt): speed_error = target_linear.x - cur_linear.x brake = 0.0 throttle = 0.0 acc = self.acc_pid.step(speed_error, dt) if speed_error < 0.0: # http://www.asawicki.info/Mirror/Car%20Physics%20for%20Games/Car%20Physics%20for%20Games.html # torque = force * wheel_radius = mass * acceleration * wheel_radius torque = abs(acc) * self.vehicle_mass * self.wheel_radius brake = torque else: throttle = acc if throttle < 0.01: throttle = 0.0 steer = self.steering_controller.get_steering(target_linear.x, target_ang.z, cur_linear.x) rospy.logdebug_throttle( 2, 'cur {}, target {}, throttle {}, brake {}, steer {}'.format( cur_linear.x, target_linear.x, throttle, brake, steer)) return throttle, brake, steer
def send_commands(self, command_struct): """ Writes the desired commands in command_struct to the device over serial. The command_struct *must* be the exact same as the struct to receive it on the other device! Currently, it is 10 bytes: lin_vel, ang_vel, base_rotate, actuator_1_move, actuator_2_move, wrist_rotate, wrist_actuator_move, gripper_move servo_yaw, servo_pitch A command to control the camera servo will most likely be added at some point, probably another byte """ rospy.logdebug_throttle(2, "sending commands: {}".format(" ".join(str(c) for c in command_struct))) assert(len(command_struct) == COMMAND_STRUCT_LEN) command_data = struct.pack(COMMAND_STRUCT_FORMAT, *command_struct) checksum = calc_checksum(command_data) try: self.serial_conn.write(BEGIN_MESSAGE_BYTE) # Send struct length - must convert len(command_data) to byte array self.serial_conn.write(bytearray((len(command_data),))) self.serial_conn.write(command_data) # Send checksum self.serial_conn.write(bytearray((checksum,))) # Regularly times out here! except serial.SerialTimeoutException: rospy.logerr("Serial write timeout") # Just pass for now, ignoring this attempt and trying again later pass
def callback(self,data): rospy.logdebug("reached callback. that means I can read the Subscriber!") rospy.set_param('~alive',1) try: cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) scores = self.net.predict_single_frame([cv_image,], 'fc-action', frame_size=(self.framesize_width, self.framesize_height)) #print((scores)) #this publishes the instant time version, aka, per frame self.label_pub.pub([scores]) rospy.logdebug("published the label for instant time version!") #this part publishes the frame_window version self.frame_scores.append(scores) if len(self.frame_scores)>self.classwindow: self.frame_scores.pop(0) self.label_fw_pub.pub(self.frame_scores) rospy.logdebug("published the label for the frame window version!") self.lock.acquire() if self.startedownvid: self.ownvidscores.append(scores) else: rospy.logdebug_throttle(20,"waiting for start_vidscores call to start classifying ownvid") self.lock.release()
def forward_kinematics(self): """ Calculate current twist of the rover given current drive and corner motor velocities Also approximate current turning radius. Note that forward kinematics means solving an overconstrained system since the corner motors may not be aligned perfectly and drive velocities might fight each other """ # calculate current turning radius according to each corner wheel's angle # corner motor angles should be flipped since different coordinate axes in this node (positive z up) theta_fl = -self.curr_positions['corner_left_front'] theta_fr = -self.curr_positions['corner_right_front'] theta_bl = -self.curr_positions['corner_left_back'] theta_br = -self.curr_positions['corner_right_back'] # sum wheel angles to find out which direction the rover is mostly turning in if theta_fl + theta_fr + theta_bl + theta_br > 0: # turning left r_front_closest = self.d1 + self.angle_to_turning_radius(theta_fl) r_front_farthest = -self.d1 + self.angle_to_turning_radius(theta_fr) r_back_closest = -self.d1 - self.angle_to_turning_radius(theta_bl) r_back_farthest = self.d1 - self.angle_to_turning_radius(theta_br) else: # turning right r_front_farthest = self.d1 + self.angle_to_turning_radius(theta_fl) r_front_closest = -self.d1 + self.angle_to_turning_radius(theta_fr) r_back_farthest = -self.d1 - self.angle_to_turning_radius(theta_bl) r_back_closest = self.d1 - self.angle_to_turning_radius(theta_br) # get a best estimate of the turning radius by taking the median value (avg sensitive to outliers) approx_turning_radius = sum(sorted([r_front_farthest, r_front_closest, r_back_farthest, r_back_closest])[1:3])/2.0 if math.isnan(approx_turning_radius): approx_turning_radius = self.max_radius rospy.logdebug_throttle(1, "Current approximate turning radius: {}".format(round(approx_turning_radius, 2))) self.curr_turning_radius = approx_turning_radius
def execute(self, userdata=None): goal_pose = self.goal_pose.resolve() if hasattr( self.goal_pose, "resolve") else self.goal_pose if self._goal_reached( *self._get_target_delta_in_robot_frame(goal_pose)): rospy.loginfo("We are already there") return 'succeeded' rate = rospy.Rate(self._rate) rospy.loginfo("Starting alignment ....") while not rospy.is_shutdown(): dx, dy, dyaw = self._get_target_delta_in_robot_frame(goal_pose) if self._goal_reached(dx, dy, dyaw): rospy.loginfo("Goal reached") return 'succeeded' rospy.logdebug_throttle( 0.1, "Aligning .. Delta = {} {} {}".format(dx, dy, dyaw)) self.robot.base.force_drive( vx=_clamp(self.params.abs_vx, self.params.position_gain * dx), vy=_clamp(self.params.abs_vy, self.params.position_gain * dy), vth=_clamp(self.params.abs_vyaw, self.params.rotation_gain * dyaw), timeout=1 / float(self._rate), loop_rate=self._rate, stop=False) rate.sleep() self.robot.base.force_drive(vx=0, vy=0, vth=0, timeout=0, stop=True) return 'failed'
def callback(data): r = numpy.array(data.ranges) # x2 stuff because LaserScan has 721 increments ~= 0.5 deg resolution back = numpy.concatenate((r[:0 + pan], r[360 * 2 - pan:])) right = r[90 * 2 - pan:90 * 2 + pan] front = r[180 * 2 - pan:180 * 2 + pan] left = r[270 * 2 - pan:270 * 2 + pan] dists = dict( back=back[numpy.nonzero(back)].mean(), right=right[numpy.nonzero(right)].mean(), front=front[numpy.nonzero(front)].mean(), left=left[numpy.nonzero(left)].mean(), ) rospy.logdebug_throttle(1, "distances\n%r" % dists) if dists[side] > door_dist: self.logger.info("open detected! dist to %r: %.3r m" % (side, dists[side])) self.cob.unregister() self.res = "open" return else: #self.res = "closed" self.logger.debug("dist to %r: %.3r m" % (side, dists[side])) pass
def _spin_once(self): """ one optimisation iteration, without rate control checks that required data are here, and calls for planification, then publishes results """ with self.orders_lock: with self.map_lock: if self.orders is None: rospy.logwarn_throttle( 10, 'Captain cannot plan path : missing orders') return False, None if self.map is None: rospy.logwarn_throttle( 10, 'Captain cannot plan path : missing map') return False, None if self.last_orders_seq == self.orders.header.seq: # orders didn't change, no need to work rospy.logdebug_throttle(1, "Captain didn't get new orders") return False, None rospy.loginfo_throttle(1, "starting planification") # let's compute ! success, results = self._planif_assign() self.last_orders_seq = self.orders.header.seq if not success: rospy.logwarn_throttle(1, "RRT search did not succeed") else: self._publish_captain_orders(results) self._publish_captain_viz(results)
def publishQTCdata(self): # this method is called by one of the other callbacks, so its under the lock already #with self.data_lock: validData = self.closest_human_pose and self.closest_human_twist and self.robotPoseSt if validData: # all should be already in global frame ... (xh0, yh0, hh0, vh0, wh0) = self.getHumanState() (xh1, yh1, hh1) = self.getNextHumanState(xh0, yh0, hh0, vh0, wh0) xr0 = self.robotPoseSt.pose.position.x yr0 = self.robotPoseSt.pose.position.y hr0 = self.getRotation(self.robotPoseSt.pose.orientation) (xr1, yr1, hr1) = self.getNextRobotState() # get state only if transforms where successfull if ((xh0!=None) and (xh1!=None) and(xr0!=None) and(xr1!=None)): (isValid, state) = self.getQSRState(xh0, yh0, xh1, yh1, xr0, yr0, xr1, yr1) self.qtc_state = state self.is_valid = isValid # Finally publish ....................... if self.is_valid: self.qtc_state_pub.publish(self.qtc_state) qtc_data = Float64MultiArray() qtc_data.data = [xh0, yh0, hh0, xh1, yh1, hh1, xr0, yr0, hr0, xr1, yr1, hr1] self.qtc_points_pub.publish(qtc_data) rospy.logdebug_throttle(1, "Node [" + rospy.get_name() + "] " + "Updated visuals ... " )
def calculate_cp(self, com_mark): current_time = com_mark.header.stamp time_difference = (current_time - self.prev_t).to_sec() if current_time is not self.prev_t: x_dot = (com_mark.pose.position.x - self.prev_x) / time_difference y_dot = (com_mark.pose.position.y - self.prev_y) / time_difference try: trans = self.tf_buffer.lookup_transform( 'world', self.foot_link, rospy.Time()) try: multiplier = sqrt(com_mark.pose.position.z / self.g) except ValueError: rospy.logdebug_throttle( 1, 'Cannot calculate capture point, because center of mass height is ' 'smaller than 0') return self.marker x_cp = trans.transform.translation.x + x_dot * multiplier y_cp = trans.transform.translation.y + y_dot * multiplier self.update_marker(x_cp, y_cp) self.prev_x = com_mark.pose.position.x self.prev_y = com_mark.pose.position.y self.prev_t = current_time except tf2_ros.TransformException as e: rospy.logdebug( 'Error in trying to lookup transform for capture point: {error}' .format(error=e)) return self.marker
def reports_callback(self, msg): with self.data_lock: nowTime = rospy.Time.now() self.prev_state = self.state self.state = msg.state self.robot_x = msg.state.position_x self.robot_y = msg.state.position_y self.robot_h = msg.state.orientation_angle self.robot_v = 0 self.robot_w = 0 if (self.prev_state and self.prev_time): inc_t = (nowTime - self.prev_time).to_sec() inc_x = self.state.position_x - self.prev_state.position_x inc_y = self.state.position_y - self.prev_state.position_y inc_r = np.sqrt((inc_x**2) + (inc_y**2)) inc_h = self.state.orientation_angle - self.prev_state.orientation_angle if (inc_t > 0): self.robot_v = inc_r / inc_t self.robot_w = inc_h / inc_t else: self.robot_v = 0 self.robot_w = 0 rospy.logdebug_throttle( 5, "Node [" + rospy.get_name() + "] " + "Robot status: Pose ( " + str(self.state.position_x) + ", " + str(self.state.position_y) + ", " + str(self.state.orientation_angle * 180.0 / np.pi) + " deg), " + "Speed ( " + str(self.robot_v) + " m/sec, " + str(self.robot_w * 180.0 / np.pi) + " deg/sec) ") self.prev_time = nowTime
def getNextRobotState(self): xr1 = yr1 = hr1 = None inc_x = inc_y = inc_h = dt = 0 if (self.prev_robotPoseSt and self.robotPoseSt and (self.robotPose_time > self.prev_time) ): t = [1, 2, 3] p = [3, 2, 0] np.interp(2.5, t, p) inc_x = self.robotPoseSt.pose.position.x - self.prev_robotPoseSt.pose.position.x inc_y = self.robotPoseSt.pose.position.y - self.prev_robotPoseSt.pose.position.y inc_h = self.getRotation(self.robotPoseSt.pose.orientation) - self.getRotation(self.prev_robotPoseSt.pose.orientation) dt = (self.sampling_time ) / (self.robotPose_time - self.prev_time).to_sec() if self.robotPoseSt: xr1 = self.robotPoseSt.pose.position.x + inc_x*dt yr1 = self.robotPoseSt.pose.position.y + inc_y*dt hr1 = self.getRotation(self.robotPoseSt.pose.orientation) + inc_h*dt rospy.logdebug_throttle(5, "Node [" + rospy.get_name() + "] " + "\n " + "Robot status: Pose 0 ( " + '{0:.2f}'.format(self.prev_robotPoseSt.pose.position.x) + ", " + '{0:.2f}'.format(self.prev_robotPoseSt.pose.position.y) + ", " + '{0:.2f}'.format(self.getRotation(self.prev_robotPoseSt.pose.orientation) *180.0/np.pi) + " deg) " + " at " + '{0:.2f}'.format(self.prev_time.to_sec() ) + " secs." + "\n" + "Robot status: Pose 1 ( " + '{0:.2f}'.format(self.robotPoseSt.pose.position.x) + ", " + '{0:.2f}'.format(self.robotPoseSt.pose.position.y) + ", " + '{0:.2f}'.format(self.getRotation(self.robotPoseSt.pose.orientation) *180.0/np.pi) + " deg) " + " at " + '{0:.2f}'.format(self.robotPose_time.to_sec() ) + " secs." + "\n" + "Robot status: Pose 2( " + '{0:.2f}'.format(xr1) + ", " + '{0:.2f}'.format(yr1) + ", " + '{0:.2f}'.format(hr1 *180.0/np.pi) + " deg) " + " at " + '{0:.2f}'.format(self.sampling_time+self.robotPose_time.to_sec() ) + " secs." + "\n" ) return (xr1, yr1, hr1)
def execute(self, ud): if self._goal_reached(*self._get_target_delta_in_robot_frame(self.goal_pose)): rospy.loginfo("We are already there") return rospy.loginfo("Starting alignment ....") while not rospy.is_shutdown(): dx, dy, dyaw = self._get_target_delta_in_robot_frame(self.goal_pose) if self._goal_reached(dx, dy, dyaw): break rospy.logdebug_throttle(0.1, "Aligning .. Delta = {} {} {}".format(dx, dy, dyaw)) self._cmd_vel_publisher.publish(Twist( linear=Vector3( x=_clamp(self.params.abs_vx, self.params.position_gain * dx), y=_clamp(self.params.abs_vy, self.params.position_gain * dy) ), angular=Vector3(z=_clamp(self.params.abs_vyaw, self.params.rotation_gain * dyaw)) )) self._rate.sleep() rospy.loginfo("Goal reached")
def updateVisuals(self): validData = self.qtc_data and self.qtc_state if validData: (xh0, yh0, hh0, xh1, yh1, hh1, xr0, yr0, hr0, xr1, yr1, hr1) = self.qtc_data tx = (xh0 + xr0) / 2.0 ty = (yh0 + yr0) / 2.0 data = MarkerArray() # connecting line (blue) .............................................. line = self.getMarker(Marker.LINE_STRIP, xh0, yh0, xr0, yr0, 0, 0, 1, "connecting_line") data.markers.append(line) # robot motion (red) ....................................................... line = self.getMarker(Marker.ARROW, xr0, yr0, xr1, yr1, 1, 0, 0, "robot_motion") data.markers.append(line) # human motion (green) ....................................................... line = self.getMarker(Marker.ARROW, xh0, yh0, xh1, yh1, 0, 1, 0, "human_motion") data.markers.append(line) # mfc should these lines be longer? # descriptive text ....................................................... text = Marker() text.id = 1 text.type = Marker.TEXT_VIEW_FACING text.header.frame_id = self.global_frame_id text.header.stamp = rospy.Time.now() text.ns = "descriptor" text.action = Marker.ADD text.pose.orientation.w = 1.0 text.pose.position.x = tx text.pose.position.y = ty text.pose.position.z = 1 + 0.2 # TEXT_VIEW_FACING markers use only the z component of scale, specifies the height of an uppercase "A". text.scale.z = 0.45 # we show next state as text text.text = (self.qtc_state.data) if (self.use_pretty_text): text.text = self.getPrettyText(text.text) # red color text.color.r = 1 text.color.g = 0.0 text.color.a = 1.0 data.markers.append(text) # Finally publish ....................... self.visual_pub.publish(data) rospy.logdebug_throttle( 1, "Node [" + rospy.get_name() + "] " + "Updated visuals ... ")
def process_program_argument(self, int16_msg): try: self.program_argument = int16_msg rospy.logdebug_throttle(5, "process_program_argument: [%d]", int16_msg.data) except Exception as e: rospy.logerr('[script_executor:process_program_id] Exception: %s', str(e))
def _controlCallback(self, msg): self.control_msg = msg err_x = self.control_msg.x.err_p err_y = self.control_msg.y.err_p err_z = self.control_msg.z.err_p err_yaw = self.control_msg.yaw.err_p err_xy = math.sqrt(err_x * err_x + err_y * err_y) rospy.logdebug_throttle( 1, 'errors in [xy, z, yaw]: [%f (%f, %f), %f, %f]' % (err_xy, err_x, err_y, err_z, err_yaw))
def callback(self,data): rospy.logdebug("reached callback. that means I can read the Subscriber!") rospy.set_param('~alive',1) try: cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: #print(e) rospy.logerr(e) #since I am not using stacks for rgb images, I can prevent from making the rgb version any slower by using an if statement here if self.rgbOrFlow == 'flow': ## and I want the combined flow version here, don't I? so I need to strip the frame apart into components. I think it is better than #self.cv_image_stack.append(cv_image) #this would be incorrect, i need to get each matrix and put it together in a stack. first x then y # from ros_flow_bg, I can see that x is the 0 component and y the 1 component. I hope bgr8 stays bgr8 and we don't flip things around here! self.cv_image_stack.append(cv_image[:,:,0]) self.cv_image_stack.append(cv_image[:,:,1]) # self.cv_image_stack.extend([cv_image[:,:,0], cv_image[:,:,1]]) if len(self.cv_image_stack)>2*self.stack_depth: #keeps at most 2*self.stack_depth images in cv_image_stack, the 2 comes from the fact that we are using flow_x and flow_y self.cv_image_stack.pop(0) self.cv_image_stack.pop(0) if self.stack_count%self.step == 0: rospy.logdebug("reached execution part of callback!") self.stack_count = 0 ## we don't keep a large number here. scores = None ### i can maybe use a lambda to abstract this. is it faster than an if though? if self.rgbOrFlow == 'rgb': scores = self.net.predict_single_frame([cv_image,], 'fc-action', frame_size=(self.framesize_width, self.framesize_height)) elif self.rgbOrFlow == 'flow' and len(self.cv_image_stack)==10: scores = self.net.predict_single_flow_stack(self.cv_image_stack, 'fc-action', frame_size=(self.framesize_width, self.framesize_height)) #print((scores)) if isinstance(scores, np.ndarray): #this publishes the instant time version, aka, per frame self.label_pub.pub([scores]) rospy.logdebug("published the label for instant time version!") #this part publishes the frame_window version self.frame_scores.append(scores) if len(self.frame_scores)>self.classwindow: self.frame_scores.pop(0) self.label_fw_pub.pub(self.frame_scores) rospy.logdebug("published the label for the frame window version!") with self.lock: if self.startedownvid: self.ownvidscores.append(scores) else: rospy.logdebug_throttle(20,"waiting for start_vidscores call to start classifying ownvid") self.stack_count = self.stack_count + 1
def forward_kinematics(self): """ Calculate current twist of the rover given current drive and corner motor velocities Also approximate current turning radius. Note that forward kinematics means solving an overconstrained system since the corner motors may not be aligned perfectly and drive velocities might fight each other """ # calculate current turning radius according to each corner wheel's angle # corner motor angles should be flipped since different coordinate axes in this node (positive z up) theta_fl = -self.curr_positions['corner_left_front'] theta_fr = -self.curr_positions['corner_right_front'] theta_bl = -self.curr_positions['corner_left_back'] theta_br = -self.curr_positions['corner_right_back'] # sum wheel angles to find out which direction the rover is mostly turning in if theta_fl + theta_fr + theta_bl + theta_br > 0: # turning left r_front_closest = self.d1 + self.angle_to_turning_radius(theta_fl) r_front_farthest = -self.d1 + self.angle_to_turning_radius( theta_fr) r_back_closest = -self.d1 - self.angle_to_turning_radius(theta_bl) r_back_farthest = self.d1 - self.angle_to_turning_radius(theta_br) else: # turning right r_front_farthest = self.d1 + self.angle_to_turning_radius(theta_fl) r_front_closest = -self.d1 + self.angle_to_turning_radius(theta_fr) r_back_farthest = -self.d1 - self.angle_to_turning_radius(theta_bl) r_back_closest = self.d1 - self.angle_to_turning_radius(theta_br) # get a best estimate of the turning radius by taking the median value (avg sensitive to outliers) approx_turning_radius = sum( sorted([ r_front_farthest, r_front_closest, r_back_farthest, r_back_closest ])[1:3]) / 2.0 if math.isnan(approx_turning_radius): approx_turning_radius = self.max_radius rospy.logdebug_throttle( 1, "Current approximate turning radius: {}".format( round(approx_turning_radius, 2))) self.curr_turning_radius = approx_turning_radius # we know that the linear velocity in x direction is the instantaneous velocity of the middle virtual # wheel which spins at the average speed of the two middle outer wheels. drive_angular_velocity = ( self.curr_velocities['drive_left_middle'] + self.curr_velocities['drive_right_middle']) / 2. self.curr_twist.twist.linear.x = drive_angular_velocity * self.wheel_radius # now calculate angular velocity from its relation with linear velocity and turning radius self.curr_twist.twist.angular.z = self.curr_twist.twist.linear.x / self.curr_turning_radius # covariance self.curr_twist.covariance = 36 * [ 0.0, ]
def getPrettyText(self, qtc_state): qtc_data = qtc_state.split(',') human_h = qtc_data[0] robot_h = qtc_data[1] human_v = qtc_data[2] robot_v = qtc_data[3] text_h = 'H:' + self.getPhrase(human_h, human_v) text_r = 'R:' + self.getPhrase(robot_h, robot_v) text = text_h + '\n' + text_r rospy.logdebug_throttle(1, "Node [" + rospy.get_name() + "] " + text) return text
def estimation(self, msg=None): self.mutex.acquire() try: hpp = self.hpp() q_current = hpp.robot.getCurrentConfig() self._initialize_constraints(q_current) # The optimization expects a configuration which already satisfies the constraints projOk, q_projected, error = hpp.problem.applyConstraints( q_current) if projOk: optOk, q_estimated, error = hpp.problem.optimize(q_projected) if not optOk: from numpy.linalg import norm errNorm = norm(error) if errNorm > 1e-2: rospy.logwarn_throttle( 1, "Optimisation failed ? error norm: {0}".format( errNorm)) rospy.logdebug_throttle( 1, "estimated == projected: {0}".format( q_projected == q_estimated)) else: rospy.loginfo_throttle( 1, "Optimisation failed ? error norm: {0}".format( errNorm)) rospy.logdebug_throttle(1, "Error {0}".format(error)) valid, msg = hpp.robot.isConfigValid(q_estimated) if not valid: rospy.logwarn_throttle( 1, "Estimation in collision: {0}".format(msg)) self.publishers["estimation"]["semantic"].publish(q_estimated) self.publish_state(hpp) else: hpp.robot.setCurrentConfig(q_current) q_estimated = q_current rospy.logwarn_throttle( 1, "Could not apply the constraints {0}".format(error)) except Exception as e: rospy.logerr_throttle(1, str(e)) rospy.logerr_throttle(1, traceback.format_exc()) finally: self.last_stamp_is_ready = False self.mutex.release()
def cmd_cb(self, twist_msg, intuitive=False): """ Respond to an incoming Twist command in one of two ways depending on the mode (intuitive) The Mathematically correct mode (intuitive=False) means that * when the linear velocity is zero, an angular velocity does not cause the corner motors to move (since simply steering the corners while standing still doesn't generate a twist) * when driving backwards, steering behaves opposite as what you intuitively might expect (this is to hold true to the commanded twist) Use this topic with a controller that generated velocities based on targets. When you're controlling the robot with a joystick or other manual input topic, consider using the /cmd_vel_intuitive topic instead. The Intuitive mode (intuitive=True) means that sending a positive angular velocity (moving joystick left) will always make the corner wheels turn 'left' regardless of the linear velocity. :param intuitive: determines the mode """ desired_turning_radius = self.twist_to_turning_radius(twist_msg, intuitive_mode=intuitive) rospy.logdebug_throttle(1, "desired turning radius: {}".format(desired_turning_radius)) corner_cmd_msg = self.calculate_corner_positions(desired_turning_radius) # if we're turning, calculate the max velocity the middle of the rover can go max_vel = abs(desired_turning_radius) / (abs(desired_turning_radius) + self.d1) * self.max_vel if math.isnan(max_vel): # turning radius infinite, going straight max_vel = self.max_vel velocity = min(max_vel, twist_msg.linear.x) rospy.logdebug_throttle(1, "velocity drive cmd: {} m/s".format(velocity)) drive_cmd_msg = self.calculate_drive_velocities(velocity, desired_turning_radius) rospy.logdebug_throttle(1, "drive cmd:\n{}".format(drive_cmd_msg)) rospy.logdebug_throttle(1, "corner cmd:\n{}".format(corner_cmd_msg)) if self.corner_cmd_threshold(corner_cmd_msg): self.corner_cmd_pub.publish(corner_cmd_msg) self.drive_cmd_pub.publish(drive_cmd_msg)
def publish(self): if not self.base_waypoints or not self.pose: return index = self.get_next_waypoint_index() waypoints = self.get_final_waypoints(index) waypoints = self.adjust_velocity(waypoints, index) final_waypoints_msg = Lane() final_waypoints_msg.waypoints = waypoints self.final_waypoints_pub.publish(final_waypoints_msg) rospy.logdebug_throttle( 1, 'Position {}, Light {}'.format(index, self.light_wp - index))
def process_program_id(self, uint16_msg): try: if uint16_msg.data == 0: rospy.logdebug_throttle(5, "Received script_id: 0 for reset") self.script_id_zero_received = True return if not self.script_id_zero_received: rospy.logdebug_throttle( 5, "Avoiding script execution because script_id: 0 has not been received yet" ) return self.script_id_zero_received = False self.execution_status_publisher.publish( UInt8(self.ExecutionStatus.RESET)) time.sleep(self.sleep_time_in_seconds_between_status_messages) self.execution_status_publisher.publish( UInt8(self.ExecutionStatus.PROCESSING)) time.sleep(self.sleep_time_in_seconds_between_status_messages) rospy.loginfo("process_program_id: [%d]", uint16_msg.data) if uint16_msg.data > 0 and uint16_msg.data <= len( self.scripts_configuration): command = str(self.scripts_configuration[uint16_msg.data - 1]) command = command.replace("#", str(self.program_argument.data)) command = command.replace("$", self.scripts_directory + "/") rospy.loginfo("command: [%s]", command) time.sleep(self.sleep_time_in_seconds_before_running_script) result_status = subprocess.call(command, shell=True, executable='/bin/bash') rospy.loginfo("command return code: [%s]", result_status) if result_status == 0: self.execution_status_publisher.publish( UInt8(self.ExecutionStatus.SUCCESS)) else: self.execution_status_publisher.publish( UInt8(self.ExecutionStatus.FAILED)) else: rospy.logerr( '[script_executor:process_program_id] Received program_id [%s] but it must be > 0 and <= size of scripts_configuration (current size: %s)', str(uint16_msg.data), str(len(self.scripts_configuration))) self.execution_status_publisher.publish( UInt8(self.ExecutionStatus.FAILED)) except Exception as e: rospy.logerr('[script_executor:process_program_id] Exception: %s', str(e)) self.execution_status_publisher.publish( UInt8(self.ExecutionStatus.FAILED))
def publish_debug_data(self, label, data): """ Publish debug data. Can be viewed using the dsd-visualization This method is safe to call without wrapping it in a try-catch block although invalid values will be wrapped in a `str()` call :type label: str :type data: dict or list or int or float or str or bool """ if type(data) not in (dict, list, int, float, str, bool): rospy.logdebug_throttle(1, "The supplied debug data of type {} is not JSON serializable and will be wrapped in str()".format(type(data))) data = str(data) rospy.logdebug('{}: {}'.format(label, data)) self._debug_data[label] = data
def reports_callback(self, msg): self.state = msg.state self.controller_status = msg.status rospy.logdebug_throttle(1,"Node [" + rospy.get_name() + "] \n- Vehicle State [" + self.status_dict['VehicleState'] + "]" + "\n- Controller State [" + self.status_dict['ControllerState'] + "]" + "\n- Control Status [" + self.status_dict['ControlStatus'] + "]" ) if (self.controller_status == ControllerReport.CONTROLLER_STATUS_WAIT) and (self.goalPS!=None): rospy.logdebug("Node [" + rospy.get_name() + "] Goal to be send in some seconds ...") rospy.sleep(5.1) rospy.logdebug("Node [" + rospy.get_name() + "] Asking for a new one ......................") self.goalPS.header.stamp = rospy.Time.now() self.goal_topic_pub.publish(self.goalPS) self.goalPS=None rospy.logdebug("Node [" + rospy.get_name() + "] Done")
def check_and_submit(self, new_position, new_rotation): """ Checks if position has changed significantly before submmitting a message """ current_translation = np.array([new_position.x, new_position.y, new_position.z]) if all(np.isclose(current_translation, self.last_translation, atol=1.e-4)): return # no significant change, abort self.last_translation = current_translation _, _, yaw = euler_from_quaternion( quaternion=(new_rotation.x, new_rotation.y, new_rotation.z, new_rotation.w)) msg = self.compose_modify_terrain_ellipse_message(new_position, degrees(yaw)) self.visual_pub.publish(msg) rospy.logdebug_throttle(1, "modify_terrain_scoop message:\n" + str(msg))
def ir_array_filter(self, data): if self.count < NUM_SAMPLE: self.ir_array_sum = [ x + y for x, y in zip(self.ir_array_sum, data.ranges) ] # print("count: " + str(self.count) + " ir_arr_sum: " + str(self.ir_array_sum)) self.count += 1 else: self.ir_array_filtered[:] = [ x / NUM_SAMPLE for x in self.ir_array_sum ] self.ir_front = self.ir_array_filtered[0:NUM_SS_FRONT] self.ir_back = self.ir_array_filtered[NUM_SS_FRONT:NUM_SENSOR] rospy.logdebug_throttle( 1, "ir_array_filtered: {}".format( self._list(self.ir_array_filtered))) self.count = 0 self.ir_array_sum = NUM_SENSOR * [0]