2/3 寻找目标出现的初始or最后位置(First Position of Target/Last Position of Target)

1 题目

  寻找目标出现的初始位置(First Position of Target)

lintcode:题号——14,难度——easy

2 描述

  给定一个排序的整数数组(升序)和一个要查找的整数 target,用O(log n)的时间查找到target第一次出现的下标(从0开始),如果target不存在于数组中,返回-1。

样例:

输入:数组 = [1,4,4,5,7,7,8,9,9,10], target = 4
输出:1

3 思路

  从头开始遍历需要的时间复杂度是O(n),要求在O(log n)的时间内完成,可以用二分法解决。

  1. 找到中间位置的元素;
  2. 与目标元素比较,确定目标元素所在的区间,缩小目标区间;
  3. 重复以上操作,直到找到或者结束。

  与之前的经典二分搜索1大致相同,可以套用经典二分搜索的模版,需要注意的差异如下:

  1. 循环中与目标元素的比较在“>”和“==”时候的处理,都是抛掉右边的部分;
  2. 跳出循环之后,对剩下的两个元素的检查顺序是从左到右。

3.2 图解

中间位置元素'7',大于目标元素'4'
中间位置元素'4',小于等于目标元素'4'
中间位置元素'4',小于等于目标元素'4'
在头尾元素中依次寻找目标元素
有序数组'1, 4, 4, 5, 7, 7, 8, 9, 9, 10' 目标元素'4'
缩小区间至'1, 4, 4, 5, 7'
抛掉'7, 8, 9, 9, 10'
缩小区间至'1, 4, 4'
抛掉'5, 7'
缩小区间至'1, 4'
找到目标元素'4'

3.2 时间复杂度

  算法的时间复杂度为O(log n)

3.3 空间复杂度

  算法的空间复杂度为O(1)

4 源码

  C++版本:

/**
* @param nums: 有序数组
* @param target: 目标元素的值
* @return: 目标元素在有序数组中的初始位置(下标序号)
*/
int binarySearch(vector<int> &nums, int target) {
    // write your code here
    if (nums.empty())
    {
        return -1;
    }
    
    int start = 0;
    int end = nums.size() - 1;
    int mid = 0;
    while (start + 1 < end)
    {
        mid = start + (end - start) / 2;
        if (nums.at(mid) >= target) // 大于和等于的情况处理方式相同
        {
            end = mid;
        }
        else
        {
            start = mid;
        }
    }
    
    if (nums.at(start) == target) // 先判断左边的是否是目标元素
    {
        return start;
    }
    if (nums.at(end) == target)
    {
        return end;
    }
    
    return -1;
}

5 相同类型问题

  题目:

寻找目标出现的最后位置(Last Position of Target)
lintcode:题号——458,难度——easy

  C++版本:

/**
* @param nums: 有序数组
* @param target: 目标元素的值
* @return: 目标元素在有序数组中的最后位置(下标序号)
*/
int lastPosition(vector<int> &nums, int target) {
    // write your code here
    if (nums.empty())
    {
        return -1;
    }
    
    int start = 0;
    int end = nums.size() - 1;
    int mid = 0;
    while (start + 1 < end)
    {
        mid = start + (end - start) / 2;
        if (nums.at(mid) <= target)
        {
            start = mid;
        }
        else
        {
            end = mid;
        }
    }
    
    if (nums.at(end) == target)
    {
        return end;
    }
    
    if (nums.at(start) == target)
    {
        return start;
    }
    
    return -1;
}

  1. 经典二分搜索:https://blog.csdn.net/SeeDoubleU/article/details/118271548 ↩︎

ros2中晃动imu,但是我的机器人模型不跟着晃动,而且里程计一开,里程计跑走,但是imu和机器人模型还在原地,以下是我的各个文件代码,情分析一下为什么:wheeltec_robot.cpp文件代码为: #include "turn_on_wheeltec_robot/wheeltec_robot.h" #include "turn_on_wheeltec_robot/Quaternion_Solution.h" #include "wheeltec_robot_msg/msg/data.hpp" sensor_msgs::msg::Imu Mpu6050;//Instantiate an IMU object //实例化IMU对象 using std::placeholders::_1; using namespace std; rclcpp::Node::SharedPtr node_handle = nullptr; //自动回充使用相关变量 bool check_AutoCharge_data = false; bool charge_set_state = false; /************************************** Date: January 28, 2021 Function: The main function, ROS initialization, creates the Robot_control object through the Turn_on_robot class and automatically calls the constructor initialization 功能: 主函数,ROS初始化,通过turn_on_robot类创建Robot_control对象并自动调用构造函数初始化 ***************************************/ int main(int argc, char** argv) { rclcpp::init(argc, argv); //ROS initializes and sets the node name //ROS初始化 并设置节点名称 turn_on_robot Robot_Control;//Instantiate an object //实例化一个对象 Robot_Control.Control();//Loop through data collection and publish the topic //循环执行数据采集和发布话题等操作 return 0; } /************************************** Date: January 28, 2021 Function: Data conversion function 功能: 数据转换函数 ***************************************/ short turn_on_robot::IMU_Trans(uint8_t Data_High,uint8_t Data_Low) { short transition_16; transition_16 = 0; transition_16 |= Data_High<<8; transition_16 |= Data_Low; return transition_16; } float turn_on_robot::Odom_Trans(uint8_t Data_High,uint8_t Data_Low) { float data_return; short transition_16; transition_16 = 0; transition_16 |= Data_High<<8; //Get the high 8 bits of data //获取数据的高8位 transition_16 |= Data_Low; //Get the lowest 8 bits of data //获取数据的低8位 data_return = (transition_16 / 1000)+(transition_16 % 1000)*0.001; // The speed unit is changed from mm/s to m/s //速度单位从mm/s转换为m/s return data_return; } /************************************** Date: January 28, 2021 Function: The speed topic subscription Callback function, according to the subscribed instructions through the serial port command control of the lower computer 功能: 速度话题订阅回调函数Callback,根据订阅的指令通过串口发指令控制下位机 ***************************************/ void turn_on_robot::Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux) { short transition; //intermediate variable //中间变量 Send_Data.tx[0]= 0x06; //frame head 0x7B //帧头0X7B //Send_Data.tx[1] = AutoRecharge; //set aside //预留位 Send_Data.tx[1] = FRAME_HEADER; Send_Data.tx[2] = 0; //set aside //预留位 //The target velocity of the X-axis of the robot //机器人x轴的目标线速度 transition=0; transition = twist_aux->linear.x*1000; //将浮点数放大一千倍,简化传输 (mm/s) Send_Data.tx[4] = transition; //取数据的低8位 Send_Data.tx[3] = transition>>8; //取数据的高8位 //The target velocity of the Y-axis of the robot //机器人y轴的目标线速度 transition=0; transition = twist_aux->linear.y*1000; Send_Data.tx[6] = transition; Send_Data.tx[5] = transition>>8; //The target angular velocity of the robot&#39;s Z axis //机器人z轴的目标角速度 transition=0; transition = twist_aux->angular.z*1000; //(1000*rad/s) Send_Data.tx[8] = transition; Send_Data.tx[7] = transition>>8; Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); //For the BCC check bits, see the Check_Sum function //BCC校验位,规则参见Check_Sum函数 Send_Data.tx[10]=0x0d; //frame tail 0x7D //帧尾0X7D Send_Data.tx[11]=0x0a; try { Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); //Sends data to the downloader via serial port //通过串口向下位机发送数据 } catch (serial::IOException& e) { RCLCPP_ERROR(this->get_logger(),("Unable to send data through serial port")); //If sending data fails, an error message is printed //如果发送数据失败,打印错误信息 } } /************************************** Date: January 28, 2021 Function: Publish the IMU data topic 功能: 发布IMU数据话题 ***************************************/ void turn_on_robot::Publish_ImuSensor() { sensor_msgs::msg::Imu Imu_Data_Pub; //Instantiate IMU topic data //实例化IMU话题数据 Imu_Data_Pub.header.stamp = rclcpp::Node::now(); Imu_Data_Pub.header.frame_id = gyro_frame_id; //IMU corresponds to TF coordinates, which is required to use the robot_pose_ekf feature pack //IMU对应TF坐标,使用robot_pose_ekf功能包需要设置此项 Imu_Data_Pub.orientation.x = Mpu6050.orientation.x; //A quaternion represents a three-axis attitude //四元数表达三轴姿态 Imu_Data_Pub.orientation.y = Mpu6050.orientation.y; Imu_Data_Pub.orientation.z = Mpu6050.orientation.z; Imu_Data_Pub.orientation.w = Mpu6050.orientation.w; Imu_Data_Pub.orientation_covariance[0] = 1e6; //Three-axis attitude covariance matrix //三轴姿态协方差矩阵 Imu_Data_Pub.orientation_covariance[4] = 1e6; Imu_Data_Pub.orientation_covariance[8] = 1e-6; Imu_Data_Pub.angular_velocity.x = Mpu6050.angular_velocity.x; //Triaxial angular velocity //三轴角速度 Imu_Data_Pub.angular_velocity.y = Mpu6050.angular_velocity.y; Imu_Data_Pub.angular_velocity.z = Mpu6050.angular_velocity.z; Imu_Data_Pub.angular_velocity_covariance[0] = 1e6; //Triaxial angular velocity covariance matrix //三轴角速度协方差矩阵 Imu_Data_Pub.angular_velocity_covariance[4] = 1e6; Imu_Data_Pub.angular_velocity_covariance[8] = 1e-6; Imu_Data_Pub.linear_acceleration.x = Mpu6050.linear_acceleration.x; //Triaxial acceleration //三轴线性加速度 Imu_Data_Pub.linear_acceleration.y = Mpu6050.linear_acceleration.y; Imu_Data_Pub.linear_acceleration.z = Mpu6050.linear_acceleration.z; imu_publisher->publish(Imu_Data_Pub); //Pub IMU topic //发布IMU话题 } /************************************** Date: January 28, 2021 Function: Publish the odometer topic, Contains position, attitude, triaxial velocity, angular velocity about triaxial, TF parent-child coordinates, and covariance matrix 功能: 发布里程计话题,包含位置、姿态、三轴速度、绕三轴角速度、TF父子坐标、协方差矩阵 ***************************************/ void turn_on_robot::Publish_Odom() { //Convert the Z-axis rotation Angle into a quaternion for expression //把Z轴转角转换为四元数进行表达 tf2::Quaternion q; q.setRPY(0,0,Mpu6050_Data.imu_yaw); geometry_msgs::msg::Quaternion odom_quat=tf2::toMsg(q); nav_msgs::msg::Odometry odom; //Instance the odometer topic data //实例化里程计话题数据 odom.header.stamp = rclcpp::Node::now(); ; odom.header.frame_id = odom_frame_id; // Odometer TF parent coordinates //里程计TF父坐标 odom.pose.pose.position.x = Robot_Pos.X; //Position //位置 odom.pose.pose.position.y = Robot_Pos.Y; //odom.pose.pose.position.z = Robot_Pos.Z; odom.pose.pose.orientation = odom_quat; //Posture, Quaternion converted by Z-axis rotation //姿态,通过Z轴转角转换的四元数 odom.child_frame_id = robot_frame_id; // Odometer TF subcoordinates //里程计TF子坐标 odom.twist.twist.linear.x = Robot_Vel.X; //Speed in the X direction //X方向速度 //odom.twist.twist.linear.y = Robot_Vel.Y; //Speed in the Y direction //Y方向速度 odom.twist.twist.angular.z = Mpu6050.angular_velocity.z; //Angular velocity around the Z axis //绕Z轴角速度 //There are two types of this matrix, which are used when the robot is at rest and when it is moving.Extended Kalman Filtering officially provides 2 matrices for the robot_pose_ekf feature pack //这个矩阵有两种,分别在机器人静止和运动的时候使用。扩展卡尔曼滤波官方提供的2个矩阵,用于robot_pose_ekf功能包 //if(Robot_Vel.X== 0&&Robot_Vel.Y== 0&&Robot_Vel.Z== 0) if(Robot_Vel.X== 0) //If the velocity is zero, it means that the error of the encoder will be relatively small, and the data of the encoder will be considered more reliable //如果velocity是零,说明编码器的误差会比较小,认为编码器数据更可靠 memcpy(&odom.pose.covariance, odom_pose_covariance2, sizeof(odom_pose_covariance2)), memcpy(&odom.twist.covariance, odom_twist_covariance2, sizeof(odom_twist_covariance2)); else //If the velocity of the trolley is non-zero, considering the sliding error that may be brought by the encoder in motion, the data of IMU is considered to be more reliable //如果小车velocity非零,考虑到运动中编码器可能带来的滑动误差,认为imu的数据更可靠 memcpy(&odom.pose.covariance, odom_pose_covariance, sizeof(odom_pose_covariance)), memcpy(&odom.twist.covariance, odom_twist_covariance, sizeof(odom_twist_covariance)); odom_publisher->publish(odom); //Pub odometer topic //发布里程计话题 } /************************************** Date: January 28, 2021 Function: Publish voltage-related information 功能: 发布电压相关信息 ***************************************/ void turn_on_robot::Publish_Voltage() { std_msgs::msg::Float32 voltage_msgs; //Define the data type of the power supply voltage publishing topic //定义电源电压发布话题的数据类型 static float Count_Voltage_Pub=0; if(Count_Voltage_Pub++>10) { Count_Voltage_Pub=0; voltage_msgs.data = Power_voltage; //The power supply voltage is obtained //电源供电的电压获取 voltage_publisher->publish(voltage_msgs); //Post the power supply voltage topic unit: V, volt //发布电源电压话题单位:V、伏特 } } ////////// 回充发布与回调 //////// /************************************** Date: January 17, 2022 Function: Pub the topic whether the robot finds the infrared signal (charging station) 功能: 发布机器人是否寻找到红外信号(充电桩)的话题 ***************************************/ void turn_on_robot::Publish_RED() { std_msgs::msg::UInt8 msg; msg.data=Red; RED_publisher->publish(msg); } /************************************** Date: January 14, 2022 Function: Publish a topic about whether the robot is charging 功能: 发布机器人是否在充电的话题 ***************************************/ void turn_on_robot::Publish_Charging() { static bool last_charging; std_msgs::msg::Bool msg; msg.data=Charging; Charging_publisher->publish(msg); if(last_charging==false && Charging==true)cout<<GREEN<<"Robot is charging."<<endl<<RESET; if(last_charging==true && Charging==false)cout<<RED <<"Robot charging has disconnected."<<endl<<RESET; last_charging=Charging; } /************************************** Date: January 28, 2021 Function: Publish charging current information 功能: 发布充电电流信息 ***************************************/ void turn_on_robot::Publish_ChargingCurrent() { std_msgs::msg::Float32 msg; msg.data=Charging_Current; Charging_current_publisher->publish(msg); } /************************************** Date: March 1, 2022 Function: Infrared connection speed topic subscription Callback function, according to the subscription command through the serial port to set the infrared connection speed 功能: 红外对接速度话题订阅回调函数Callback,根据订阅的指令通过串口发指令设置红外对接速度 ***************************************/ void turn_on_robot::Red_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux) { short transition; //intermediate variable //中间变量 Send_Data.tx[0]=FRAME_HEADER; //frame head 0x7B //帧头0X7B Send_Data.tx[1] = 3; //Infrared docking speed setting flag bit = 3 //红外对接速度设置标志位=3 Send_Data.tx[2] = 0; //set aside //预留位 //The target velocity of the X-axis of the robot //机器人x轴的目标线速度 transition=0; transition = twist_aux->linear.x*1000; //将浮点数放大一千倍,简化传输 Send_Data.tx[4] = transition; //取数据的低8位 Send_Data.tx[3] = transition>>8; //取数据的高8位 //The target velocity of the Y-axis of the robot //机器人y轴的目标线速度 transition=0; transition = twist_aux->linear.y*1000; Send_Data.tx[6] = transition; Send_Data.tx[5] = transition>>8; //The target angular velocity of the robot&#39;s Z axis //机器人z轴的目标角速度 transition=0; transition = twist_aux->angular.z*1000; Send_Data.tx[8] = transition; Send_Data.tx[7] = transition>>8; Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); //BCC check //BCC校验 Send_Data.tx[10]=FRAME_TAIL; //frame tail 0x7D //帧尾0X7D try { Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); //Sends data to the downloader via serial port //通过串口向下位机发送数据 } catch (serial::IOException& e) { RCLCPP_ERROR(this->get_logger(),("Unable to send data through serial port")); //If sending data fails, an error message is printed //如果发送数据失败,打印错误信息 } } /************************************** Date: January 14, 2022 Function: Subscription robot recharge flag bit topic, used to tell the lower machine speed command is normal command or recharge command 功能: 订阅机器人是否回充标志位话题,用于告诉下位机速度命令是正常命令还是回充命令 ***************************************/ void turn_on_robot::Recharge_Flag_Callback(const std_msgs::msg::Int8::SharedPtr Recharge_Flag) { AutoRecharge=Recharge_Flag->data; } //服务 void turn_on_robot::Set_Charge_Callback(const shared_ptr<turtlesim::srv::Spawn::Request> req,shared_ptr<turtlesim::srv::Spawn::Response> res) { Send_Data.tx[0]=FRAME_HEADER; //frame head 0x7B //֡ͷ0X7B if(round(req->x)==1) Send_Data.tx[1] = 1; else if(round(req->x)==2) Send_Data.tx[1] = 2; else if(round(req->x)==0) Send_Data.tx[1] = 0,AutoRecharge=0; Send_Data.tx[2] = 0; Send_Data.tx[3] = 0; Send_Data.tx[4] = 0; Send_Data.tx[5] = 0; Send_Data.tx[6] = 0; Send_Data.tx[7] = 0; Send_Data.tx[8] = 0; Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); //For the BCC check bits, see the Check_Sum function //BCCУ��λ������μ�Check_Sum���� Send_Data.tx[10]=FRAME_TAIL; //frame tail 0x7D //֡β0X7D try { Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); //Sends data to the downloader via serial port //ͨ����������λ���������� } catch (serial::IOException& e) { res->name = "false"; } if( Send_Data.tx[1]==0 ) { if(charge_set_state==0) AutoRecharge=0,res->name = "true"; else res->name = "false"; } else { if(charge_set_state==1) res->name = "true"; else res->name = "false"; } return; } ////////// 回充发布与回调 //////// /************************************** Date: January 28, 2021 Function: Serial port communication check function, packet n has a byte, the NTH -1 byte is the check bit, the NTH byte bit frame end.Bit XOR results from byte 1 to byte n-2 are compared with byte n-1, which is a BCC check Input parameter: Count_Number: Check the first few bytes of the packet 功能: 串口通讯校验函数,数据包n有个字节,第n-1个字节为校验位,第n个字节位帧尾。第1个字节到第n-2个字节数据按位异或的结果与第n-1个字节对比,即为BCC校验 输入参数: Count_Number:数据包前几个字节加入校验 mode:对发送数据还是接收数据进行校验 ***************************************/ unsigned char turn_on_robot::Check_Sum(unsigned char Count_Number,unsigned char mode) { unsigned char check_sum=0,k; if(mode==0) //Receive data mode //接收数据模式 { for(k=0;k<Count_Number;k++) { check_sum=check_sum^Receive_Data.rx[k]; //By bit or by bit //按位异或 } } if(mode==1) //Send data mode //发送数据模式 { for(k=1;k<Count_Number;k++) { check_sum=check_sum^Send_Data.tx[k]; //By bit or by bit //按位异或 } } return check_sum; //Returns the bitwise XOR result //返回按位异或结果 } //自动回充专用校验位 unsigned char turn_on_robot::Check_Sum_AutoCharge(unsigned char Count_Number,unsigned char mode) { unsigned char check_sum=0,k; if(mode==0) //Receive data mode //接收数据模式 { for(k=0;k<Count_Number;k++) { check_sum=check_sum^Receive_AutoCharge_Data.rx[k]; //By bit or by bit //按位异或 } } return check_sum; } /************************************** Date: November 18, 2021 Function: Read and verify the data sent by the lower computer frame by frame through the serial port, and then convert the data into international units 功能: 通过串口读取并逐帧校验下位机发送过来的数据,然后数据转换为国际单位 ***************************************/ bool turn_on_robot::Get_Sensor_Data_New() { short transition_16=0; //Intermediate variable //中间变量 //uint8_t i=0; uint8_t check=0,check2=0, error=1,error2=1,Receive_Data_Pr[1]; //Temporary variable to save the data of the lower machine //临时变量,保存下位机数据 static int count,count2; //Static variable for counting //静态变量,用于计数 Stm32_Serial.read(Receive_Data_Pr,sizeof(Receive_Data_Pr)); //Read the data sent by the lower computer through the serial port //通过串口读取下位机发送过来的数据 /*//View the received raw data directly and debug it for use//直接查看接收到的原始数据,调试使用 ROS_INFO("%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x", Receive_Data_Pr[0],Receive_Data_Pr[1],Receive_Data_Pr[2],Receive_Data_Pr[3],Receive_Data_Pr[4],Receive_Data_Pr[5],Receive_Data_Pr[6],Receive_Data_Pr[7], Receive_Data_Pr[8],Receive_Data_Pr[9],Receive_Data_Pr[10],Receive_Data_Pr[11],Receive_Data_Pr[12],Receive_Data_Pr[13],Receive_Data_Pr[14],Receive_Data_Pr[15], Receive_Data_Pr[16],Receive_Data_Pr[17],Receive_Data_Pr[18],Receive_Data_Pr[19],Receive_Data_Pr[20],Receive_Data_Pr[21],Receive_Data_Pr[22],Receive_Data_Pr[23]); */ Receive_Data.rx[count] = Receive_Data_Pr[0]; //Fill the array with serial data //串口数据填入数组 Receive_AutoCharge_Data.rx[count2] = Receive_Data_Pr[0]; Receive_Data.Frame_Header = Receive_Data.rx[0]; //The first part of the data is the frame header 0X7B //数据的第一位是帧头0X7B Receive_Data.Frame_Tail = Receive_Data.rx[23]; //The last bit of data is frame tail 0X7D //数据的最后一位是帧尾0X7D //接收到自动回充数据的帧头、上一个数据是24字节的帧尾,表明自动回充数据开始到来 if((Receive_Data_Pr[0] == AutoCharge_HEADER )||count2>0) count2++; else count2=0; if(Receive_Data_Pr[0] == FRAME_HEADER || count>0) //Ensure that the first data in the array is FRAME_HEADER //确保数组第一个数据为FRAME_HEADER count++; else count=0; //自动回充数据处理 if(count2 == AutoCharge_DATA_SIZE) { count2=0; if(Receive_AutoCharge_Data.rx[AutoCharge_DATA_SIZE-1]==AutoCharge_TAIL) //确认帧尾 { check2 = Check_Sum_AutoCharge(6,0);//校验位计算 if(check2 == Receive_AutoCharge_Data.rx[AutoCharge_DATA_SIZE-2]) //校验正确 { error2=0; } if(error2 == 0) //校验正确开始赋值 { transition_16 = 0; transition_16 |= Receive_AutoCharge_Data.rx[1]<<8; transition_16 |= Receive_AutoCharge_Data.rx[2]; Charging_Current = transition_16/1000+(transition_16 % 1000)*0.001; //充电电流 Red = Receive_AutoCharge_Data.rx[3]; //红外接受状态 Charging = Receive_AutoCharge_Data.rx[4];//小车充电状态 charge_set_state = Receive_AutoCharge_Data.rx[5]; check_AutoCharge_data = true; //数据成功接收标志位 } } } if(count == 24) //Verify the length of the packet //验证数据包的长度 { count=0; //Prepare for the serial port data to be refill into the array //为串口数据重新填入数组做准备 if(Receive_Data.Frame_Tail == FRAME_TAIL) //Verify the frame tail of the packet //验证数据包的帧尾 { check=Check_Sum(22,READ_DATA_CHECK); //BCC check passes or two packets are interlaced //BCC校验通过或者两组数据包交错 if(check == Receive_Data.rx[22]) { error=0; //XOR bit check successful //异或位校验成功 } if(error == 0) { /*//Check receive_data.rx for debugging use //查看Receive_Data.rx,调试使用 ROS_INFO("%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x", Receive_Data.rx[0],Receive_Data.rx[1],Receive_Data.rx[2],Receive_Data.rx[3],Receive_Data.rx[4],Receive_Data.rx[5],Receive_Data.rx[6],Receive_Data.rx[7], Receive_Data.rx[8],Receive_Data.rx[9],Receive_Data.rx[10],Receive_Data.rx[11],Receive_Data.rx[12],Receive_Data.rx[13],Receive_Data.rx[14],Receive_Data.rx[15], Receive_Data.rx[16],Receive_Data.rx[17],Receive_Data.rx[18],Receive_Data.rx[19],Receive_Data.rx[20],Receive_Data.rx[21],Receive_Data.rx[22],Receive_Data.rx[23]); */ Receive_Data.Flag_Stop=Receive_Data.rx[1]; //set aside //预留位 Robot_Vel.X = Odom_Trans(Receive_Data.rx[2],Receive_Data.rx[3]); //Get the speed of the moving chassis in the X direction //获取运动底盘X方向速度 //Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]); //Get the speed of the moving chassis in the Y direction, The Y speed is only valid in the omnidirectional mobile robot chassis //获取运动底盘Y方向速度,Y速度仅在全向移动机器人底盘有效 //Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]); //Get the speed of the moving chassis in the Z direction //获取运动底盘Z方向速度 //MPU6050 stands for IMU only and does not refer to a specific model. It can be either MPU6050 or MPU9250 //Mpu6050仅代表IMU,不指代特定型号,既可以是MPU6050也可以是MPU9250 Mpu6050_Data.imu_rol = IMU_Trans(Receive_Data.rx[4],Receive_Data.rx[5]) * 0.01 * 3.14159/180; Mpu6050_Data.imu_yaw = IMU_Trans(Receive_Data.rx[6],Receive_Data.rx[7]) * 0.01 * 3.14159/180; Mpu6050_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8],Receive_Data.rx[9]); //Get the X-axis acceleration of the IMU //获取IMU的X轴加速度 Mpu6050_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10],Receive_Data.rx[11]); //Get the Y-axis acceleration of the IMU //获取IMU的Y轴加速度 Mpu6050_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12],Receive_Data.rx[13]); //Get the Z-axis acceleration of the IMU //获取IMU的Z轴加速度 Mpu6050_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14],Receive_Data.rx[15]); //Get the X-axis angular velocity of the IMU //获取IMU的X轴角速度 Mpu6050_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16],Receive_Data.rx[17]); //Get the Y-axis angular velocity of the IMU //获取IMU的Y轴角速度 Mpu6050_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18],Receive_Data.rx[19]); //Get the Z-axis angular velocity of the IMU //获取IMU的Z轴角速度 //Linear acceleration unit conversion is related to the range of IMU initialization of STM32, where the range is ±2g=19.6m/s^2 //线性加速度单位转化,和STM32的IMU初始化的时候的量程有关,这里量程±2g=19.6m/s^2 Mpu6050.linear_acceleration.x = Mpu6050_Data.accele_x_data * 0.01 * 9.8; Mpu6050.linear_acceleration.y = Mpu6050_Data.accele_y_data * 0.01 * 9.8; Mpu6050.linear_acceleration.z = Mpu6050_Data.accele_z_data * 0.01 * 9.8; //The gyroscope unit conversion is related to the range of STM32&#39;s IMU when initialized. Here, the range of IMU&#39;s gyroscope is ±500°/s //Because the robot generally has a slow Z-axis speed, reducing the range can improve the accuracy //陀螺仪单位转化,和STM32的IMU初始化的时候的量程有关,这里IMU的陀螺仪的量程是±500°/s //因为机器人一般Z轴速度不快,降低量程可以提高精度 Mpu6050.angular_velocity.x = Mpu6050_Data.gyros_x_data * 0.01 *3.14159/180; Mpu6050.angular_velocity.y = Mpu6050_Data.gyros_y_data * 0.01 *3.14159/180; Mpu6050.angular_velocity.z = Mpu6050_Data.gyros_z_data * 0.01 *3.14159/180; //Get the battery voltage //获取电池电压 transition_16 = 0; transition_16 |= Receive_Data.rx[20]<<8; transition_16 |= Receive_Data.rx[21]; Power_voltage = transition_16/1000+(transition_16 % 1000)*0.001; //Unit conversion millivolt(mv)->volt(v) //单位转换毫伏(mv)->伏(v) return true; } } } return false; } /************************************** Date: January 28, 2021 Function: Loop access to the lower computer data and issue topics 功能: 循环获取下位机数据与发布话题 ***************************************/ void turn_on_robot::Control() { //_Last_Time = ros::Time::now(); _Last_Time = rclcpp::Node::now(); while(rclcpp::ok()) { try { //_Now = ros::Time::now(); _Now = rclcpp::Node::now(); Sampling_Time = (_Now - _Last_Time).seconds(); //Retrieves time interval, which is used to integrate velocity to obtain displacement (mileage) //获取时间间隔,用于积分速度获得位移(里程) if (true == Get_Sensor_Data_New()) //The serial port reads and verifies the data sent by the lower computer, and then the data is converted to international units //通过串口读取并校验下位机发送过来的数据,然后数据转换为国际单位 { //Robot_Pos.X+=(Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * Sampling_Time; //Calculate the displacement in the X direction, unit: m //计算X方向的位移,单位:m //Robot_Pos.Y+=(Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * Sampling_Time; //Calculate the displacement in the Y direction, unit: m //计算Y方向的位移,单位:m //Robot_Pos.Z+=Robot_Vel.Z * Sampling_Time; //The angular displacement about the Z axis, in rad //绕Z轴的角位移,单位:rad Robot_Pos.X+=(Robot_Vel.X * cos(Mpu6050_Data.imu_yaw)) * Sampling_Time; //Calculate the displacement in the X direction, unit: m //计算X方向的位移,单位:m Robot_Pos.Y+=(Robot_Vel.X * sin(Mpu6050_Data.imu_yaw)) * Sampling_Time; //Calculate the displacement in the Y direction, unit: m //计算Y方向的位移,单位:m //Robot_Pos.Z = 0 ; //Calculate the three-axis attitude from the IMU with the angular velocity around the three-axis and the three-axis acceleration //通过IMU绕三轴角速度与三轴加速度计算三轴姿态 Quaternion_Solution(Mpu6050.angular_velocity.x, Mpu6050.angular_velocity.y, Mpu6050.angular_velocity.z,\ Mpu6050.linear_acceleration.x, Mpu6050.linear_acceleration.y, Mpu6050.linear_acceleration.z); Publish_Odom(); //Pub the speedometer topic //发布里程计话题 Publish_ImuSensor(); //Pub the IMU topic //发布IMU话题 Publish_Voltage(); //Pub the topic of power supply voltage //发布电源电压话题 _Last_Time = _Now; //Record the time and use it to calculate the time interval //记录时间,用于计算时间间隔 } //自动回充数据话题 if(check_AutoCharge_data) { Publish_Charging(); //Pub a topic about whether the robot is charging //发布机器人是否在充电的话题 Publish_RED(); //Pub the topic whether the robot finds the infrared signal (charging station) //发布机器人是否寻找到红外信号(充电桩)的话题 Publish_ChargingCurrent(); //Pub the charging current topic //发布充电电流话题 check_AutoCharge_data = false; } rclcpp::spin_some(this->get_node_base_interface()); //The loop waits for the callback function //循环等待回调函数 } catch (const rclcpp::exceptions::RCLError & e ) { RCLCPP_ERROR(this->get_logger(),"unexpectedly failed whith %s",e.what()); } } } /************************************** Date: January 28, 2021 Function: Constructor, executed only once, for initialization 功能: 构造函数, 只执行一次,用于初始化 ***************************************/ turn_on_robot::turn_on_robot():rclcpp::Node ("wheeltec_robot") { Sampling_Time=0; Power_voltage=0; //Clear the data //清空数据 memset(&Robot_Pos, 0, sizeof(Robot_Pos)); memset(&Robot_Vel, 0, sizeof(Robot_Vel)); memset(&Receive_Data, 0, sizeof(Receive_Data)); memset(&Send_Data, 0, sizeof(Send_Data)); memset(&Mpu6050_Data, 0, sizeof(Mpu6050_Data)); //ros::NodeHandle private_nh("~"); //Create a node handle //创建节点句柄 //The private_nh.param() entry parameter corresponds to the initial value of the name of the parameter variable on the parameter server //private_nh.param()入口参数分别对应:参数服务器上的名称 参数变量名 初始值 this->declare_parameter<int>("serial_baud_rate",115200); this->declare_parameter<std::string>("usart_port_name", "/dev/ttyACM0"); this->declare_parameter<std::string>("odom_frame_id", "odom_combined"); this->declare_parameter<std::string>("robot_frame_id", "base_footprint"); this->declare_parameter<std::string>("gyro_frame_id", "gyro_link"); this->get_parameter("serial_baud_rate", serial_baud_rate);//Communicate baud rate 115200 to the lower machine //和下位机通信波特率115200 this->get_parameter("usart_port_name", usart_port_name);//Fixed serial port number //固定串口号 this->get_parameter("odom_frame_id", odom_frame_id);//The odometer topic corresponds to the parent TF coordinate //里程计话题对应父TF坐标 this->get_parameter("robot_frame_id", robot_frame_id);//The odometer topic corresponds to sub-TF coordinates //里程计话题对应子TF坐标 this->get_parameter("gyro_frame_id", gyro_frame_id);//IMU topics correspond to TF coordinates //IMU话题对应TF坐标 odom_publisher = create_publisher<nav_msgs::msg::Odometry>("odom", 2);//Create the odometer topic publisher //创建里程计话题发布者 imu_publisher = create_publisher<sensor_msgs::msg::Imu>("imu/data_raw", 2); //Create an IMU topic publisher //创建IMU话题发布者 voltage_publisher = create_publisher<std_msgs::msg::Float32>("PowerVoltage", 1);//Create a battery-voltage topic publisher //创建电池电压话题发布者 //回充发布者 Charging_publisher = create_publisher<std_msgs::msg::Bool>("robot_charging_flag", 10); Charging_current_publisher = create_publisher<std_msgs::msg::Float32>("robot_charging_current", 10); RED_publisher = create_publisher<std_msgs::msg::UInt8>("robot_red_flag", 10); //回充订阅者 Red_Vel_Sub = create_subscription<geometry_msgs::msg::Twist>( "red_vel", 10, std::bind(&turn_on_robot::Red_Vel_Callback, this, std::placeholders::_1)); Recharge_Flag_Sub = create_subscription<std_msgs::msg::Int8>( "robot_recharge_flag", 10, std::bind(&turn_on_robot::Recharge_Flag_Callback, this,std::placeholders::_1)); //回充服务提供 SetCharge_Service=this->create_service<turtlesim::srv::Spawn>\ ("/set_charge",std::bind(&turn_on_robot::Set_Charge_Callback,this, std::placeholders::_1 ,std::placeholders::_2)); //Set the velocity control command callback function //速度控制命令订阅回调函数设置 Cmd_Vel_Sub = create_subscription<geometry_msgs::msg::Twist>( "cmd_vel", 2, std::bind(&turn_on_robot::Cmd_Vel_Callback, this, _1)); RCLCPP_INFO(this->get_logger(),"wheeltec_robot Data ready"); //Prompt message //提示信息 try { //Attempts to initialize and open the serial port //尝试初始化与开启串口 Stm32_Serial.setPort(usart_port_name); //Select the serial port number to enable //选择要开启的串口号 Stm32_Serial.setBaudrate(serial_baud_rate); //Set the baud rate //设置波特率 serial::Timeout _time = serial::Timeout::simpleTimeout(2000); //Timeout //超时等待 Stm32_Serial.setTimeout(_time); Stm32_Serial.open(); //Open the serial port //开启串口 } catch (serial::IOException& e) { RCLCPP_ERROR(this->get_logger(),"wheeltec_robot can not open serial port,Please check the serial port cable! "); //If opening the serial port fails, an error message is printed //如果开启串口失败,打印错误信息 } if(Stm32_Serial.isOpen()) { RCLCPP_INFO(this->get_logger(),"wheeltec_robot serial port opened"); //Serial port opened successfully //串口开启成功提示 } } /************************************** Date: January 28, 2021 Function: Destructor, executed only once and called by the system when an object ends its life cycle 功能: 析构函数,只执行一次,当对象结束其生命周期时系统会调用这个函数 ***************************************/ turn_on_robot::~turn_on_robot() { //Sends the stop motion command to the lower machine before the turn_on_robot object ends //对象turn_on_robot结束前向下位机发送停止运动命令 Send_Data.tx[0]=0x06; Send_Data.tx[1] = FRAME_HEADER; Send_Data.tx[2] = 0; //The target velocity of the X-axis of the robot //机器人X轴的目标线速度 Send_Data.tx[4] = 0; Send_Data.tx[3] = 0; //The target velocity of the Y-axis of the robot //机器人Y轴的目标线速度 Send_Data.tx[6] = 0; Send_Data.tx[5] = 0; //The target velocity of the Z-axis of the robot //机器人Z轴的目标角速度 Send_Data.tx[8] = 0; Send_Data.tx[7] = 0; Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); //Check the bits for the Check_Sum function //校验位,规则参见Check_Sum函数 Send_Data.tx[10]=0x0d; Send_Data.tx[11]=0x0a; try { Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx)); //Send data to the serial port //向串口发数据 } catch (serial::IOException& e) { RCLCPP_ERROR(this->get_logger(),"Unable to send data through serial port"); //If sending data fails, an error message is printed //如果发送数据失败,打印错误信息 } Stm32_Serial.close(); //Close the serial port //关闭串口 RCLCPP_INFO(this->get_logger(),"Shutting down"); //Prompt message //提示信息 } turn_on_wheeltec_robot.launch.py文件代码为: import os from pathlib import Path import launch from launch.actions import SetEnvironmentVariable from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import (DeclareLaunchArgument, GroupAction, IncludeLaunchDescription, SetEnvironmentVariable) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import PushRosNamespace import launch_ros.actions from launch.conditions import IfCondition from launch.conditions import UnlessCondition def generate_launch_description(): # Get the launch directory bringup_dir = get_package_share_directory(&#39;turn_on_wheeltec_robot&#39;) launch_dir = os.path.join(bringup_dir, &#39;launch&#39;) ekf_config = Path(get_package_share_directory(&#39;turn_on_wheeltec_robot&#39;), &#39;config&#39;, &#39;ekf.yaml&#39;) ekf_carto_config = Path(get_package_share_directory(&#39;turn_on_wheeltec_robot&#39;), &#39;config&#39;, &#39;ekf_carto.yaml&#39;) imu_config = Path(get_package_share_directory(&#39;turn_on_wheeltec_robot&#39;), &#39;config&#39;, &#39;imu.yaml&#39;) carto_slam = LaunchConfiguration(&#39;carto_slam&#39;, default=&#39;false&#39;) carto_slam_dec = DeclareLaunchArgument(&#39;carto_slam&#39;,default_value=&#39;false&#39;) wheeltec_robot = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, &#39;base_serial.launch.py&#39;)), ) robot_ekf = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, &#39;wheeltec_ekf.launch.py&#39;)), launch_arguments={&#39;carto_slam&#39;:carto_slam}.items(), ) base_to_link = launch_ros.actions.Node( package=&#39;tf2_ros&#39;, executable=&#39;static_transform_publisher&#39;, name=&#39;base_to_link&#39;, arguments=[&#39;0&#39;, &#39;0&#39;, &#39;0&#39;,&#39;0&#39;, &#39;0&#39;,&#39;0&#39;,&#39;base_footprint&#39;,&#39;base_link&#39;], ) base_to_gyro = launch_ros.actions.Node( package=&#39;tf2_ros&#39;, executable=&#39;static_transform_publisher&#39;, name=&#39;base_to_gyro&#39;, arguments=[&#39;0&#39;, &#39;0&#39;, &#39;0&#39;,&#39;0&#39;, &#39;0&#39;,&#39;0&#39;,&#39;base_link&#39;,&#39;gyro_link&#39;], ) imu_filter_node = launch_ros.actions.Node( package=&#39;imu_filter_madgwick&#39;, executable=&#39;imu_filter_madgwick_node&#39;, parameters=[imu_config] ) # joint_state_publisher_node = launch_ros.actions.Node( # package=&#39;joint_state_publisher&#39;, # executable=&#39;joint_state_publisher&#39;, # name=&#39;joint_state_publisher&#39;, # ) #select a robot model,the default model is mini_mec #minibot.launch.py contains commonly used robot models #launch_arguments choices:mini_mec/mini_akm/mini_tank/mini_4wd/mini_diff/mini_omni/brushless_senior_diff #!!!At the same time, you need to modify ld.add_action(minibot_type) and #ld.add_action(flagship_type) minibot_type = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, &#39;robot_mode_description_minibot.launch.py&#39;)), launch_arguments={&#39;bike_robot&#39;: &#39;true&#39;}.items(), ) #robot_mode_description.launch.py contains flagship products, usually larger chassis robots #launch_arguments choices: #senior_akm/top_akm_bs/top_akm_dl #senior_mec_bs/senior_mec_dl/top_mec_bs/top_mec_dl/ mec_EightDrive_robot/flagship_mec_bs_robot/flagship_mec_dl_robot�� #senior_omni/top_omni #senior_4wd_bs_robot/senior_4wd_dl_robot/flagship_4wd_bs_robot/flagship_4wd_dl_robot/top_4wd_bs_robot/top_4wd_dl_robot #senior_diff_robot/four_wheel_diff_bs/four_wheel_diff_dl/flagship_four_wheel_diff_bs_robot/flagship_four_wheel_diff_dl_robot #!!!At the same time, you need to modify ld.add_action(flagship_type) and #ld.add_action(minibot_type) flagship_type = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, &#39;robot_mode_description.launch.py&#39;)), launch_arguments={&#39;senior_akm&#39;: &#39;true&#39;}.items(), ) ld = LaunchDescription() ld.add_action(minibot_type) #ld.add_action(flagship_type) ld.add_action(carto_slam_dec) ld.add_action(wheeltec_robot) ld.add_action(base_to_link) ld.add_action(base_to_gyro) # ld.add_action(joint_state_publisher_node) ld.add_action(imu_filter_node) ld.add_action(robot_ekf) return ld base_serial.launch.py文件代码为: from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch.conditions import IfCondition from launch.conditions import UnlessCondition import launch_ros.actions #def launch(launch_descriptor, argv): def generate_launch_description(): return LaunchDescription([ launch_ros.actions.Node( package=&#39;turn_on_wheeltec_robot&#39;, executable=&#39;wheeltec_robot_node&#39;, output=&#39;screen&#39;, parameters=[{&#39;usart_port_name&#39;: &#39;/dev/ttyACM0&#39;, &#39;serial_baud_rate&#39;:115200, &#39;robot_frame_id&#39;: &#39;base_footprint&#39;, &#39;odom_frame_id&#39;: &#39;odom_combined&#39;, &#39;cmd_vel&#39;: &#39;cmd_vel&#39;,}], ) ]) robot_mode_description_minibot.launch.py文件代码为: import os from ament_index_python.packages import get_package_share_directory import launch_ros.actions from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import LoadComposableNodes from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode from nav2_common.launch import RewrittenYaml def generate_robot_node(robot_urdf,child): return launch_ros.actions.Node( package=&#39;robot_state_publisher&#39;, executable=&#39;robot_state_publisher&#39;, name=f&#39;robot_state_publisher_{child}&#39;, arguments=[os.path.join(get_package_share_directory(&#39;wheeltec_robot_urdf&#39;), &#39;urdf&#39;, robot_urdf)], ) def generate_static_transform_publisher_node(translation, rotation, parent, child): return launch_ros.actions.Node( package=&#39;tf2_ros&#39;, executable=&#39;static_transform_publisher&#39;, name=f&#39;base_to_{child}&#39;, arguments=[translation[0], translation[1], translation[2], rotation[0], rotation[1], rotation[2], parent, child], ) def generate_launch_description(): bike_robot = LaunchConfiguration(&#39;bike_robot&#39;, default=&#39;false&#39;) bike_robot_ = GroupAction( condition=IfCondition(bike_robot), actions=[ generate_robot_node(&#39;xuan_bike_robot.urdf&#39;,&#39;bike_robot&#39;), generate_static_transform_publisher_node([&#39;0.048&#39;, &#39;0&#39;, &#39;0.18&#39;], [&#39;0&#39;, &#39;0&#39;, &#39;0&#39;], &#39;base_link&#39;, &#39;laser&#39;), generate_static_transform_publisher_node([&#39;0.048&#39;, &#39;0&#39;, &#39;0.18&#39;], [&#39;0&#39;, &#39;0&#39;, &#39;0&#39;], &#39;base_link&#39;, &#39;camera_link&#39;), ]) # Create the launch description and populate ld = LaunchDescription() # Set environment variables # Declare the launch options #ld.add_action(declare_use_composition_cmd) # Add the actions to launch all of the localiztion nodes ld.add_action(mini_mec_) ld.add_action(mini_akm_) ld.add_action(mini_tank_) ld.add_action(mini_4wd_) ld.add_action(mini_diff_) ld.add_action(brushless_senior_diff_) ld.add_action(bike_robot_) return ld xuan_bike_robot.urdf文件代码为: <?xml version="1.0"?> <robot name="wheeltec_robot"> <link name="base_link"> <visual> <geometry> <mesh filename="file:///home/deepbot/ros-relate/wheeltec_ros2/src/wheeltec_robot_urdf/wheeltec_robot_urdf/meshes/Xuan-release.stl"scale="0.001 0.001 0.001"/> </geometry> <origin xyz="0 0 0" rpy="0 0 0"/> <material name="blue"> <color rgba="0 0 0.8 0.5"/> </material> </visual> </link> </robot>
最新发布
05-14
在Python中,如果你想要在排序数组中查找特定元素的第一个和最后一个出现位置,可以使用类似于二分查找的思想,但不是直接的二分查找,因为我们需要分别查找开始和结束点。这里有一个简单的实现方法: 1. **初始化**: - 设置两个指针,`start`指向数组的起始位置(0),`end`指向数组的末尾位置(数组长度减1)。 2. **查找起始位置**: - 使用循环,当`start`小于等于`end`时,执行以下步骤: - 计算中间位置 `mid = (start + end) // 2`。 - 如果中间元素等于目标值,检查其左侧的元素,如果也等于目标值则移动`end`到`mid - 1`;否则,`start`移到`mid + 1`。 3. **查找结束位置**: - 类似于查找起始位置的过程,但是当找到目标值后,检查其右侧的元素,如果也等于目标值则移动`start`到`mid + 1`;否则,保持`end`不变。 4. **返回结果**: - 当`start`超过`end`时,说明找不到目标值,返回`None`或相应的位置表示不存在。否则,返回`start`作为起始位置,`end`作为结束位置。 这是基于数组有序的条件,如果数组无序,可能需要先对数组进行排序再进行上述查找。 ```python def find_first_and_last(nums, target): start, end = 0, len(nums) - 1 while start <= end: mid = (start + end) // 2 if nums[mid] == target: left_start = mid while mid > 0 and nums[mid - 1] == target: mid -= 1 return left_start, mid elif nums[mid] < target: start = mid + 1 else: end = mid - 1 return None, None # 示例 nums = [1, 2, 3, 3, 3, 4, 5] target = 3 first_pos, last_pos = find_first_and_last(nums, target) if first_pos is not None: print(f"First occurrence at position {first_pos}, Last occurrence at position {last_pos}") else: print(f"{target} not found in the array") ```
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值