问:
Rospy: Why does the loop not get interrupted?
I am working on the software for an autonomous Raspberry-Pi based robot which should keep moving for 2 minutes unless an unintended key is fed to it as input. Upon receipt of the unintended key, the robot must stop.

I appropriated the code from this tutorial and it looks like this (I removed some lines for brevity):

```<code>#Obstacle avoidance
HowNear= 15
timeLimit= 2
dt = rospy.Duration(secs=120)
reversetime= 0.5
turntime= 0.75

stop= False

#Set pins as output and input
GPIO.setup(pinTrigger, GPIO.OUT) #Trigger
GPIO.setup(pinEcho, GPIO.IN)    #Echo

# Set the GPIO Pin mode to be Output
GPIO.setup(pinMotorAForwards, GPIO.OUT)
GPIO.setup(pinMotorABackwards, GPIO.OUT)
GPIO.setup(pinMotorBForwards, GPIO.OUT)
GPIO.setup(pinMotorBBackwards, GPIO.OUT)

# Set the GPIO to software PWM at 'Frequency' Hertz
pwmMotorAForwards = GPIO.PWM(pinMotorAForwards, Frequency)
pwmMotorABackwards = GPIO.PWM(pinMotorABackwards, Frequency)
pwmMotorBForwards = GPIO.PWM(pinMotorBForwards, Frequency)
pwmMotorBBackwards = GPIO.PWM(pinMotorBBackwards, Frequency)

# Start the software PWM with a duty cycle of 0 (i.e. not moving)
pwmMotorAForwards.start(Stop)
pwmMotorABackwards.start(Stop)
pwmMotorBForwards.start(Stop)
pwmMotorBBackwards.start(Stop)

def myhook():
        print "shutting down"

# Turn all motors off
def stopmotors():
    pwmMotorAForwards.ChangeDutyCycle(Stop)
    pwmMotorABackwards.ChangeDutyCycle(Stop)
    pwmMotorBForwards.ChangeDutyCycle(Stop)
    pwmMotorBBackwards.ChangeDutyCycle(Stop)

# Turn both motors forwards
def forwards():
    pwmMotorAForwards.ChangeDutyCycle(DutyCycle)
    pwmMotorABackwards.ChangeDutyCycle(Stop)
    pwmMotorBForwards.ChangeDutyCycle(DutyCycle)
    pwmMotorBBackwards.ChangeDutyCycle(Stop)

def isnearobstacle(localhownear):
        distance= measure()

        print("IsNearObstacle: " + str(distance))
        if distance < localhownear:
                return True
        else:
                return False

def measure():
        GPIO.output(pinTrigger, True)
        time.sleep(0.00001)
        GPIO.output(pinTrigger, False)
        starttime= time.time()
        stoptime= starttime

        while GPIO.input(pinEcho)== 0:
                starttime= time.time()
                stoptime= starttime

        while GPIO.input(pinEcho)== 1:
                stoptime= time.time()
                if stoptime - starttime >= 0.04:
                        print("Too close/too far!")
                        stoptime= starttime
                        break

        elapsedtime= stoptime - starttime
        distance= (elapsedtime * 34300)/2

        return distance

def avoidobstacle():
        print("Backwards")
        backwards()
        time.sleep(reversetime)
        stopmotors()

        print("Right")
        right()
        time.sleep(turntime)
        stopmotors()

def PathScanning():
        GPIO.output(pinTrigger, False)
        time.sleep(0.1)

        globalStartTime= rospy.Time.now()
        globalStopTime= globalStartTime

        try:
                while True:
                   while not rospy.is_shutdown():
                        if stop == True:
                                print(stop)
                                stopmotors()
                        elif (globalStopTime-globalStartTime) < dt:
                                globalStopTime= rospy.Time.now()
                                forwards()
                                time.sleep(0.1)
                                if isnearobstacle(HowNear):
                                    stopmotors()
                                    avoidobstacle()
        except KeyboardInterrupt:
               print("Key Press Detected. Stopping Motors")
                stopmotors()

# Message handler
def CommandCallback(commandMessage):
        command = commandMessage.data
        if command == 'A':
                PathScanning()
        else:
                print('Unknown command, stopping instead')
                stop = True
                print(stop)
                stopmotors()

rospy.init_node('driver', anonymous=True)

rospy.Subscriber('command', String, CommandCallback)

rospy.spin()
print('Shutting down: stopping motors')

stopmotors()

GPIO.cleanup()
</code>```

The problematic portion of the code is as follows:

```<code>   def PathScanning():
            GPIO.output(pinTrigger, False)
            time.sleep(0.1)

            globalStartTime= rospy.Time.now()
            globalStopTime= globalStartTime

            try:
                    while True:
                       while not rospy.is_shutdown():
                            if stop == True:
                                    print(stop)
                                    stopmotors()
                            elif (globalStopTime-globalStartTime) < dt:
                                    globalStopTime= rospy.Time.now()
                                    forwards()
                                    time.sleep(0.1)
                                    if isnearobstacle(HowNear):
                                        stopmotors()
                                        avoidobstacle()
            except KeyboardInterrupt:
                   print("Key Press Detected. Stopping Motors")
                    stopmotors()

    # Message handler
    def CommandCallback(commandMessage):
            command = commandMessage.data
            if command == 'A':
                    PathScanning()
            else:
                    print('Unknown command, stopping instead')
                    stop = True
                    print(stop)
                    stopmotors()

    rospy.init_node('driver', anonymous=True)

    rospy.Subscriber('command', String, CommandCallback)
</code>```

My goal is to stop the robot from moving when the user passes any string/character besides 'A' as input. However, that does not happen when the PathScanning() function is already running. When I type any string besides 'A', I see the print statements on the console but stopmotors() is not called and the robot keeps moving (if 'A' was pressed earlier). I believe this is because PathScanning() has a loop in it:

```<code>   def PathScanning():
            GPIO.output(pinTrigger, False)
            time.sleep(0.1)

            globalStartTime= rospy.Time.now()
            globalStopTime= globalStartTime

            try:
                    while True:
                       while not rospy.is_shutdown():
                            if stop == True:
                                    print(stop)
                                    stopmotors()
                            elif (globalStopTime-globalStartTime) < dt:
                                    globalStopTime= rospy.Time.now()
                                    forwards()
                                    time.sleep(0.1)
                                    if isnearobstacle(HowNear):
                                        stopmotors()
                                        avoidobstacle()
            except KeyboardInterrupt:
                   print("Key Press Detected. Stopping Motors")
                    stopmotors()
</code>```

When I remove the loop from PathScanning(), the robot stops when I provide any input besides 'A'. However, my other requirement of making the robot run the PathScanning() function for exactly two minutes is not fulfilled.

How is it possible that when I give any input besides 'A', the print statements in the branch are appropriately recognized but the stopmotors() function is not called?

答:
Look in CommandCallback, where you set "stop = True". That sets a local variable, not the global variable you intended. Declare the variable to be global by preceding the assignment with the declaration "global stop".

This contrasts with reading a global variable, which just works without declaring it (notably, in PathScanning). It might not be a classic Python gotcha, but it has snagged me enough times that I look for it. One ref is https://www.geeksforgeeks.org/global-keyword-in-python/


问:
Kalman Filter and the state noise vector?
I'm reading Probabilistic Robotics by Thrun. In the Kalman filter section, they state that
$$
x_{t} =A_{t}x_{t-1} + B_{t}u_{t} + \epsilon_{t}
$$

where $\epsilon_{t}$ is the state noise vector.  And in
$$
z_{t} = C_{t}x_{t} + \delta_{t}
$$
where $\delta_{t}$ is the measurement noise. Now, I want to simulate a system in Matlab. Everything to me is straightforward except the state noise vector $\epsilon_{t}$. Unfortunately, majority of authors don't care much about the technical details. My question is what is the state noise vector? and what are the sources of it? I need to know because I want my simulation to be rather sensible. About the measurement noise, it is evident and given in the specifications sheet that is the sensor has uncertainty ${\pm} e$.

答:
In my understanding, $\epsilon_{t}$ accounts for the uncertainties of the state model. Uncertainties come from real life imperfections, for example the wheels are not completely round, or the weight distribution is not even, or the motors don't perform exactly as predicted by the model.

So when the robot executes a straight movement, it is expected to eventually reach a nearby position of that predicted by the model, a bit to the left or right.

ContentLicense: CC BY-SA 3.0


问:
Inverse kinematics Calculation for 3dof robotic arm
I am doing a project, to draw images provided using robotic arm. Initially edge detection is done on image to be drawn and coordinate value is obtained as (x,y).
How to calculate 3 joint angles inverse kinematics from this value.

答:
One way you could do it is by creating a hashmap or a LUT(lookup table).

Create an empty hashmap.

Iterate through all possible combinations of all thetas (this results in 3 for-loops) and find the resultant (x,y) coordinates.

Use the coordinates as the key to your hashmap and the angles as the value.

If the length of your robotic arm is constant, you get a better performance in speed after the initial overhead of loading/creating the LUT.

ContentLicense: CC BY-SA 4.0


问:
Unstable principal point estimation in zoom camera calibration
I have a camera with a 1/2.9" image sensor from FLIR and a 12-50mm motorized zoom lens from Theia. Because the lens is a motorized zoom lens, I tried to calibrate the camera for different zoom and focus setups with an asymmetric circle matrix board and OpenCV calibrateCamera function. Each time I calibrate for a specific zoom and focus level, both zoom and focus are fixed.

Ideally, the principal point position (cx, cy) should be close to (720, 540) because the image resolution is 1440 x 1080. When the focal length is 12mm and 50mm, cx and cy look good, less than 60 pixels away from the center. However, they vary a few hundred pixels for some middle focal length settings, such as (834, 546), (913, 596), and (949, 613), but the image center seems to be stable when the lens is zooming and should not move for a few hundred pixels. The other camera intrinsic parameters also look good.

From the minimum to the maximum focal length, I observed that the distortion status changes from barrel distortion to pincushion distortion. From my understanding, there is a range that provides the minimum distortion. The status is also that the less distortion the image has, the worse the principal position is found.

Could anyone provide me any suggestions on solving this unstable principal point estimation problem?

答:
It is very natural to have unstable principal estimation with low distortion.

I don't see any other option rather than just defining spline-based continuous time parameter for your zoom lens and give some smoothness constraint around the focal length where the lens distortion is low. This wouldn't take too much time if you are adapt to system modeling and some optimization tools like Ceres.

But I guess it wouldn't make too much difference. Anyway, your optimizer is giving varying principal locations as they don't affect the residual much.


问:
How to use knowledge of sensor noise
Reasoning about the meaning of uncertainty of sensor measurement I ran into a doubt:

(1) When we talk about sensor uncertainty, we often model it as a Gaussian distribution $\mathcal{N}(\boldsymbol{z}, \Sigma)$, where $\boldsymbol{z}$ is the value measured by the sensor and the covariance matrix $\Sigma$ gives us information about the uncertainty. In particular, $\Sigma$ represents the uncertainty on "how" far the true value $\boldsymbol{x}$ is from $\boldsymbol{z}$. The distribution is centered on $\boldsymbol{z}$ as we do not have access to the true value $\boldsymbol{x}$.

(2) Now, assume that we have access to the ground truth, and we learn the distribution of the error, such that $\boldsymbol{z} = \boldsymbol{x} + \boldsymbol{\epsilon}$ where $\boldsymbol{\epsilon} \sim \mathcal{N}(0, \hat{\Sigma})$.

The two situations might look the same, but in (1) the distribution $\mathcal{N}(\boldsymbol{z}, \Sigma)$ is centered on the measured value, while in (2) $\hat{\Sigma}$ is the covariance of a Gaussian centered on the true value. Is my interpretation of (1) correct? Can we set $\Sigma = \hat{\Sigma}$, i.e. can we use the covariance learned in (2) in the setting described in (1)?

答:
It may be worth noting that $\Sigma$ does not just describe "how far" the true value $\boldsymbol{x}$ is from $\boldsymbol{z}$ , but "where" in relation to it, the difference is apparent when the distribution is not 'circular' in the multidimensional case.

Also, a Gaussian is a symmetric distribution, and only under that condition the same $\Sigma$ can be used in (1) and (2).

If the model was not a symmetric distribution (implying it is not a Gaussian), e.g. in a 1D case the distribution would require mirroring along the vertical axis between cases (1) and (2): if $\boldsymbol{z}$ is more likely to be to the left of the true $\boldsymbol{x}$ than to the right, the inferred $\boldsymbol{x}$ would be more likely to be to the right of the observed $\boldsymbol{z}$ ...

Title: How to use knowledge of sensor noise


问:
Significance of fixed_frame parameter in lookupTransform function
I was going through the implementation of the lookupTransform function defined inside the base_local_planner::getGoalPose() function.

What is the significance of the fixed_frame parameter in tf2_ros::Buffer::lookupTransform (target_frame, target_time, source_frame, source_time, fixed_frame, timeout)  function ?

The documentation mentions the following - Get the transform between two frames by frame ID assuming fixed frame.

What's preventing us to always go for the other implementation of the lookupTransform function -   tf2_ros::Buffer::lookupTransform (target_frame, source_frame, time, timeout) ?

答:
An example use case: "For example if one wants to know what was the position of the right arm two seconds ago relative to the position the left arm has now"

Here we have two different time stamps, and in order to calculate the two different positions at these times, we need a separate reference frame:

```<code>lookupTransform("LeftArmFrame", t, "RightArmFrame", t-Duration(2), "RobotFrame");
</code>```

Body:
An example use case: "For example if one wants to know what was the position of the right arm two seconds ago relative to the position the left arm has now"

Here we have two different time stamps, and in order to calculate the two different positions at these times, we need a separate reference frame:

lookupTransform("LeftArmFrame", t, "RightArmFrame", t-Duration(2), "RobotFrame");


问:
Reflective surface
Have you encountered the problem of reflective surfaces for realsense camera (d435 or t265) and lidar (A1 or A2). I would like to know what are the ways to solve problems with reflective surfaces for them programmatically, without using other sensors such as sonars. Perhaps there are any solutions at OpenCV. I would be very glad to hear people who tried to solve a similar problem. Thank you for attention</p>

答: I had the same difficulty about 10 years ago and ended up using lacquer spray to get rid of reflectivity. This is impossible to solve edge case unless you somehow introduce semantic scene understanding and make a special routine to handle it.</p>

Title: Reflective surface


问:
Why does a motor does not run when a resistor is connected in series between the supply?
I decided to remove the battery (1.6 V 1600 mAh) from my trimmer and to power it via an old mobile charging cable (6 V 800 mAh).

So I built a voltage divider circuit, which gave me nearly 1.6 V but when connecting to the motor, the motor does not run.

When I connected the charger directly to motor it runs well.

So I got confused and connected a series resistor across the line, and the motor doesn't run (I thought that the resistor should reduce the voltage and the motor should run, but it doesn't).

From this I got to know that the current is insufficient to run the motor.

Why is this happening?

I simulated a similar circuit in TinkerCAD

Why does the reading shows 500 mV instead of 4.5 V?

答:
You are assuming that your voltage divider will split the 9 volts into 4.5V and 4.5V, but this relies on the upper resistance and the lower resistance values being equal.

The issue you've got is that your motor is in parallel with one of your resistors. The formula for total resistance of resistors in parallel is:

$$
R_{total} = \frac{1}{\frac{1}{R_1} + \frac{1}{R_2} + ...} \\
$$

Your resistors there appear to be brown-black-red-gold, which would indicate 5% tolerance 1 kOhm resistors. Larger motors have very low resistances, on the order of a few Ohms or less. Hobby motors have a higher resistance, but even 60 mA at 3V would mean the motor resistance is 50 Ohms.

So, if you're using $R_1 = 1000$ and $R_2 = 50$, then you wind up with $\frac{1}{1000} = 0.001$ and $\frac{1}{50} = 0.02$, and so your $R_{total} = \frac{1}{0.021}$, which is about 47.6 Ohms.

So now one leg of your voltage divider is 1000 Ohms, but the other leg is 47.6 Ohms. This means that your motor leg is getting $\left(\frac{47.6}{1047.6}\right)9V=0.409V$ while the other leg is getting $\left(\frac{1000}{1047.6}\right)9V=8.591V$. These numbers are approximate based on guessing your motor resistance as 50 ohms and the battery voltage as exactly 9.0 volts, but you can see that they're in the right ballpark for what you're seeing.

If you're wanting to have the voltage to your motor be 4.5 volts, then I would suggest removing the upper resistor in your voltage divider and setting the lower resistor equal to the motor resistance, again about 50 Ohms.

Note that removing the first resistor and setting the lower resistor to a lower value will result in your net resistance being 100 Ohms (or double your motor resistance), which in turn leads to a (9/100) = 90 mA current draw from the battery. This is also the 4.5 volts across your 50 ohm motor; (4.5/50) = 90 mA.

The only way to get by with keeping your voltage divider is to have the motor resistance about an order of magnitude (10x) higher than the voltage divider leg, so the motor is "negligible" compared to the resistance of the divider. In that case, if your motor resistance is 50 Ohms, you should shoot for a voltage divider that's comprised of two 5 Ohm resistors.

If you used a voltage divider made of two 5 Ohm resistors, then your $R_{total}$ for the upper leg becomes $R_{total} = \frac{1}{\frac{1}{50}+\frac{1}{5}} = \frac{1}{0.02 + 0.2} = \frac{1}{0.22} = 4.55 \Omega$.

Then your actual motor voltage becomes $\left(\frac{4.55}{9.55}\right)9V = 4.29V$, which is pretty close to your desired 4.5 Volts, but now your total resistance is 9.55 Ohms and your system is drawing 942 mA.


问:
Will wiring a unipolar stepper to a bipolar stepper driver decrease the holding torque?
I have read that you can wire a unipolar stepper to a bipolar driver, which I have, by ignoring the two extra wires. One concern I have is whether connecting a unipolar stepper to a bipolar driver will cause it to lose torque (holding or operating)?

Will it be the same? Increase?

I've read that bipolars are more bang for your buck energy-wise, and since you can "transform" a unipolar stepper to a bipolar good enough that the driver will still work right, I would think that it might run more efficiently. Is this true?

答:
From wikipedia http://en.wikipedia.org/wiki/Stepper_motor

Because windings are better utilized, they are more powerful than a unipolar motor of the

same weight. This is due to the physical space occupied by the windings. A unipolar motor
has twice the amount of wire in the same space, but only half used at any point in time,
hence is 50% efficient (or approximately 70% of the torque output available). Though a
bipolar stepper motor is more complicated to drive, the abundance of driver chips means
this is much less difficult to achieve.

If you read specifications for a stepper motor that can be wired as unipolar/bipolar, you will confirm this.

But if you have a 5 wire unipolar, the central one is common to the center tap of both coils. You need to remove this connection to use it as bipolar. If you have a 6 wire, yes it is meant for that, just ignore the two other wires (some motors have connectors so you can disconnect the wires), and run it as bipolar.

From http://www.esuli.it/2011/10/11/converting-an-unipolar-stepper-to-bipolar/


问:
Is the translational vector (positon) in transformation matrices in cm or another unit (MATLAB)?
My transformation matrix gives me, for example, x = 0.06, y = 0.27 and z = -0.04
It does not make a lot of sense to me if this is in cm.</p>

答: They are in whatever units the kinematic analysis uses.  It could be feet, inches, meters, millimeters, or whatever.  Your question is a bit like asking “what unit is the Pythagorean theorem written in?”  It just needs to be consistent with the analytic approach.</p>


问:
How to convert vertical motion to horizontal?
I am interested in using this miniature motor Squiggle Micro Motor to create very tiny horizontal movements. However, due to very limited space, I can only place it vertically within my project.

Assuming this motor is placed as follows, how can one adapt it to simultaneous movement at a right angle? (Ideally with the X-axis movement matched to the Y-axis movement as well as possible.)

答:
This should have probably been a comment to @RobertEnglish answer ... but comments don't seem to allow pictures

Bavel gears. Plastic ones should be rather inexpensive.


问:
Count number of collisions while navigating to goal
I have the navigation stack working in ROS Kinetic on my computer. In Stage ROS simulations, I can specify the goal and the robot navigates to it. I need to count the number of collisions (if any) that occurred as it traverses to the goal. How can I go about achieving this ?</p>

答: Collisions with? If you have an occupancy map, then you can check with that and the robot's base_link. If the objects are static then just having their positions will be enough to check the collisions. It completely depends on the robot and the application.


问:
Some doubts regarding SLAM and its technical terms

I have some questions regarding SLAM and some general technical terms used in the context of it.

Q1) **Online SLAM vs Offline SLAM**. Can "Online" SLAM be also called "real-time" SLAM? What is the use of offline SLAM if it can't be used on a real robot in real-time?

Q2) **Datasets for SLAM**: Are there datasets different for offline and online slam algorithms? What is the difference exactly?

答:
Q1) Online SLAM vs Offline SLAM. Can "Online" SLAM be also called "real-time" SLAM? What is the use of offline SLAM if it can't be used on a real robot in real-time?

Yes, online SLAM could probably also be called real-time SLAM as it needs to run in real-time, although you could run offline SLAM in real-time too, just not at the time the data was recorded. Not every single part of the system will necessarily run at a high frequency though. It is common for the map to be updated less frequently than the current localisation estimate, as (re)calculating the map is often the work of some larger optimisation problem, whereas the localisation can produce estimates from 'old' maps that are good enough for short periods of time (e.g. seconds) using just odometry sources, which tend to be computationally cheaper and therefore possible at higher frequency.

Offline SLAM could be used for testing different algorithms, without being restricted to keeping things real-time. Testing on the same dataset is also very useful when one wants to compare the results.
Or for making maps of an area, potentially at a higher quality than a real-time system since it does not need to take performance into account (within reason).

Q2) Datasets for SLAM: Are there datasets different for offline and online slam algorithms? What is the difference exactly?

No. Whether a dataset can be run in real-time will depend on the computational requirements of whatever SLAM system is using it plus the capabilities of the machine being used.


问:
How can I automatically adjust PID parameters on the fly?
I have a simple servo system that uses a PID controller implemented in an MCU to perform the feedback. However, the properties of the system change dynamically, and so the PID parameters can never be tuned for all circumstances.

My robot is a light weight arm with back-drivable electric motors, similar to this one:

The arm performs several tasks, including picking up heavy weights, pushing and pulling objects across the desk. Each of these tasks requires different PID tuning parameters which I cannot easily predict.

What I would really like is for some higher level function which can carefully adjust the parameters in response to the arm's behaviour. For example, if it notices that the arm is oscillating, it might reduce P and increase D. Or if it noticed that the arm wasn't reaching its target, it might increase I.

Do such algorithms exist? I would be happy even if the algorithm didn't perfect the parameters immediately. E.G. the arm could oscillate a few times before the parameters were adjusted to their new values.

答:
A co-worker and I once implemented a simplex algorithm for on-the-fly tuning of the PID parameters of a current control loop for a motor. Essentially the algorithm would modify one parameter at a time and then collect data on some feedback parameter that was our measure of goodness. Ours was percent deviation from a current target setpoint. Based on whether the feedback parameter got better or worse, the next parameter was modified accordingly.

Or, in Wikipedia speak:

Let a linear program be given by a canonical tableau. The simplex
  algorithm proceeds by performing successive pivot operations which
  each give an improved basic feasible solution; the choice of pivot
  element at each step is largely determined by the requirement that
  this pivot does improve the solution.

Technically we used the Nelder-Mead method which is a type of simplex. It could also be described as a hill climbing algorithm as well if you watch how it modifies it's input parameters as it searches for an optimum output parameter.

Nedler-Mead worked best in our case because it can chase a setpoint. This was important because our current target setpoint changed as torque demand increased.

the Nelder–Mead technique is a heuristic search method that can
  converge to non-stationary points


问:
GPS+IMU sensor fusion not based on Kalman Filters
Do you know any papers on or implementations of GPS + IMU sensor fusion for localization that are not based on an EKF (Extended Kalman Filter) or UKF (Unscented Kalman Filter)?

I'm asking is because

I've found KFs difficult to implement

I want something simpler (less computationally expensive)

rlabbe's book on Kalman Filters suggests they aren't ideal for this use case. In this chapter it says:

Kalman filters for inertial systems are very difficult, but fusing data from two or more sensors providing measurements of the same state variable (such as position) is quite easy.

Further in the "Can you Filter GPS outputs?" part

Hence, the signal is not white, it is not time independent, and if you pass that data into a Kalman filter you have violated the mathematical requirements of the filter. So, the answer is no, you cannot get better estimates by running a KF on the output of a commercial GPS. [...] This is a difficult problem that hobbyists face when trying to integrate GPS, IMU's and other off the shelf sensors.

If the above is true, other approaches should be out there. Yet answers to gps-imu fusion and implementation questions only point to EKFs. So far, I've only found a Master's thesis implementing an EIF (Extended Information Filter) and a paper with an Adaptive filter I don't quite understand. Do you know of alternative approaches?

答:
I still can't understand how the kalman filter works, but maybe you could substitute stochastic approximations for it.

Assume frequent response measurements on a static point on the GPS to obtain, for example, a Gaussian shape.

Integrate the IMU variables twice, including the error
tolerance, so that it also behave stochastic shape. you could
approximate the IMU offset error to a radius of a Gaussian.

Overlapping both frequencies results will give you a distribution of the final position, then you could use confidence intervals or take the espected value for the position.

So the result will be a distribution shape that could estimate the expected point with some confidence '$ \alpha $' (let's say 95% zone, or 99% confidence in this zone)


答:
As @SteveO says, it could be any of them.

The units are linear mappable, then you could simply multiply for a scalar and change the units and the descriptions will be the same, even you could use percentage as a unit.

So in virtual frames (on simulations) the units has not to be definited, you could interprete the "neutral" units as you wish... say that the robot was 50% smaller than the real one, then if you simply multiply any position or the axis by 2 you will get the real scalar representation.

If you have x = 0.06, y = 0.27 and z = -0.04, and you say that 0.06 means meters, then all that you will get in future calculations will be in meters, but if you change your initial consideration and say 0.06 is inch then all the remainming calculations will be in inchs too.

Title: Is the translational vector (positon) in transformation matrices in cm or another unit (MATLAB)?

Body:
My transformation matrix gives me, for example, x = 0.06, y = 0.27 and z = -0.04
It does not make a lot of sense to me if this is in cm.</p>


问:
Calculation of error, based on camera images
I am trying to measure the precision error of a gripper that is mounted to the flange of a 6 DOF industrial robot. The gripper-design is rather complex, therefore we would like to make sure it is operating precisely.

I think a simple way to measure the gripper-precision is to use a camera and visual markers on a piece that is gripped. See the following illustration of the setup. The colorful box mocks a part that is being gripped by the gray gripper that is mounted to the robot flange.

To get data to work with, I imagine the following procedure:

the robot picks up part 1

the robot holds the red side of the part in front of the camera, which takes a picture

the robot rotates the part by actuating its 6th joint by 90 degrees, the camera takes a picture of the green side

the robot rotates the part by another 90 degrees, the camera takes a picture (back side)

the robot rotates the part by another 90 degrees, the camera takes a picture

the robot places the part back in the nest

go back to step 1, until 10 iterations are completed

In summary: Each time the robot gripped the part, a batch of pictures from different sides are taken.

The error, that needs to be calculated, is defined as the pose-difference of the part that is caused by small errors in the gripper mechanism. The error has 6 dimensions.

The camera can be assumed to be calibrated.

Using two batches of pictures from two different grips: How can I use the image plane coordinates of the $N$ features attached to a part, to calculate the pose-difference of the part?

答:
You could use the batch images from each color to correlate between them (only the mask of the correspondient color).

The correlation should be 1 if all the images are exactly the same, and less than 1 if they are different. so you could associate this "cost function" to map the result coefficient to the error of the repetibility.

Also you could apply correlation between landmark of the images, like borders, how much change this border position over the baches.

But the error will have the units on this new "cost function" you can't map it to milimiters without a mapping reference. Maybe you could associate each pixel with a length, but the preferible will be take at least one better resolution measurement to "calibrate" in a milimiters units.

I think that we could see each pixel as a frequency sample, so, by nyquist you will required at least the double of pixel by milimeter resolution.

For example, if you wish resolution of 7milimiters on the repeatability, then each pixel should be capable of capture less than 3.5mm


答:
Kalman filters are just an intelligent way to do a weighted average of two measurements. Intelligent in the sense that it takes into account the uncertainties of each measurement to output the estimate with the minimum variance. In fact, if the measurement and process covariances are not changing during operation, the KF converges to a fixed-weight weighted average, e.g. a complementary filter.

You can fuse GPS position and velocity estimates with a process model and IMU orientation estimation using a weighted average.

Title: GPS+IMU sensor fusion not based on Kalman Filters

Body:
Do you know any papers on or implementations of GPS + IMU sensor fusion for localization that are not based on an EKF (Extended Kalman Filter) or UKF (Unscented Kalman Filter)?

I'm asking is because

I've found KFs difficult to implement

I want something simpler (less computationally expensive)

rlabbe's book on Kalman Filters suggests they aren't ideal for this use case. In this chapter it says:

Kalman filters for inertial systems are very difficult, but fusing data from two or more sensors providing measurements of the same state variable (such as position) is quite easy.

Further in the "Can you Filter GPS outputs?" part

Hence, the signal is not white, it is not time independent, and if you pass that data into a Kalman filter you have violated the mathematical requirements of the filter. So, the answer is no, you cannot get better estimates by running a KF on the output of a commercial GPS. [...] This is a difficult problem that hobbyists face when trying to integrate GPS, IMU's and other off the shelf sensors.

If the above is true, other approaches should be out there. Yet answers to gps-imu fusion and implementation questions only point to EKFs. So far, I've only found a Master's thesis implementing an EIF (Extended Information Filter) and a paper with an Adaptive filter I don't quite understand. Do you know of alternative approaches?


问:
How to disable GPS in ROS/MAVROS and sent the points myself?
I'm trying to create a simple MAVROS node that takes off, goes to a specific location and then comes back to its home location. As this is currently being tested inside Gazeboit uses fake gps. I now want to publish the GPS coordinates myself, and thus need to disable the existing fake gps that keeps publishing.

I could publish my gps locations (lat, lon, alt) through mavros/global_position/global topic and then read from it. I want to disable that and see if my quadrotor does go to a custom gps point that I send it to.

I also noticed there is something called mocap=true in rosparam list. Should I disable this as well or I'd be fine to leave it as is? if so how can I do that as well?

Note:

The GPS coordinates that I'm publishing are not something random. They are created using a combination of other sensors. so they are accurate, maybe not as accurate as the original GPS coordinates, but I believe they are accurate enough. that's why I asked, how I can disable the fake gps that gazebo/mavros/ros(not sure which one) is also publishing, so I can test my generated coordinates properly.

In other words, I want the mavros/global_position/global to only contain my published coordinates. currently, in gazebo/mavros there is a fake gps that also is publishing coordinates on this topic whenever I start simulating. I want to stop this

答: There will be a slight problem with that because the custom GPS point you give might be far from the current GPS coordinate at which the drone is present. I'm assuming you are using px4 autopilot(or ardupilot). I think you cannot publish to /mavros/global_position/global. You should publish your setpoints to /mavros/setpoint_position/global topic. So when you switch to offboard mode, then takeoff and publish your setpoint to the above topic. Then the drone will reach the position of your input. You can try to see the drone's home position using the QGroundControl app and then adjust your custom setpoint accordingly. Hope it helps.


问:
Why does the quadcopter become unstable after turning?
I'm trying to write a simple test MAVROS node where my quadrotor is expected to go in a straight line and then have 180-degree turn and come back and land on where it initially took off.

The issue that I'm facing now is, if I suddenly change the yaw, it shocks the quadrotor (creates quiet a bit of turbulence) and thus I want to have as smooth of a turn as possible.

So for this I thought, maybe hovering in the last position for a couple of iterations would do the trick. However, I noticed after adding this snippet, the drone flies unstably, shakes badly and the last turn before landing, seems to not to take place properly as well!

What am I missing here?

Here are two video clips I recorded 1 shows the quadrotor with my loiter_fn(hover in place for some iterations before changing the yaw), and the other without it.

Clip1 : https://files.fm/f/4ts4b4kdv

Clip2: https://files.fm/f/687gmhj6d

and here is my code :

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <tf/LinearMath/Quaternion.h>

// https://docs.px4.io/master/en/simulation/ros_interface.html

mavros_msgs::State current_state;

void state_cb(const mavros_msgs::State::ConstPtr &msg)
{
    current_state = *msg;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");

    // The setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);

    // Wait for FCU connection
    while (ros::ok() && !current_state.connected)
    {
        ros::spinOnce();
        rate.sleep();
    }

    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 0;

    auto to_degree = [](const double &radian_angle)
    {
        return radian_angle * (180 / 3.14);
    };
    auto to_radian = [](const double &degree_angle)
    {
        return (degree_angle * 3.14) / 180;
    };

    tf::Quaternion qtr;
    float yaw = to_radian(180.0);
    qtr.setRPY(0, 0, yaw);
    pose.pose.orientation.w = qtr.getW();
    pose.pose.orientation.x = qtr.getX();
    pose.pose.orientation.y = qtr.getY();
    pose.pose.orientation.z = qtr.getZ();

    // Send a few setpoints before starting
    for (int i = 100; ros::ok() && i > 0; --i)
    {
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();

    bool land = false;
    int i = 0;
    int starting_point = 0;
    float angles[] = {180.0, 360.0};

    // Hovers inplace for 'counter' iterations
    auto loiter_fn = [&](const int& wait_counter)
    {
        pose.pose.orientation.x = 1;
        for (size_t i = 0; i < wait_counter; i++)
        {
            local_pos_pub.publish(pose);
        }
    };

        // Changes the yaw
    auto change_yaw_angle = [&](const float &angle_degree, const int& wait_counter=5)
    {
        loiter_fn(wait_counter);
        qtr.setRPY(0, 0, to_radian(angle_degree));
        pose.pose.orientation.w = qtr.getW();
        pose.pose.orientation.x = qtr.getX();
        pose.pose.orientation.y = qtr.getY();
        pose.pose.orientation.z = qtr.getZ();
    };

    while (ros::ok())
    {
        if (current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0)))
        {
            if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
            {
                ROS_INFO("Offboard enabled");
                std::cout << "Offboard enabled!\n";
                starting_point = i;
            }

            std::cout << "current_state.mode: " << current_state.mode << std::endl;

            last_request = ros::Time::now();
        }
        else
        {
            if (!current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0))) //5
            {
                if (arming_client.call(arm_cmd) && arm_cmd.response.success)
                {
                    ROS_INFO("Vehicle armed");
                    std::cout << "Vehicle armed!\n";
                }
                last_request = ros::Time::now();
            }
        }

        std::cout << "status: " << current_state.mode << " " << i << "\n";
        if (current_state.mode == "OFFBOARD")
        {
            pose.pose.position.x = i;
            pose.pose.position.y = 0;
            auto z = pose.pose.position.z;
            pose.pose.position.z = z < 30 ? 0.5 * i : 30;
            pose.pose.orientation.x = 5;

            if (i > starting_point + 400)
            {
                change_yaw_angle(180);
                // since counter is not a good measure, we have to manually set an
                // offset that resembles home
                pose.pose.position.x = -(i - (starting_point + 552));
                std::cout << "status(returning): " << current_state.mode << " x: " << pose.pose.position.x << "\n";
                if (i == 800)
                {
                    change_yaw_angle(360);
                    land = true;
                }
            }
        }

        i++;
        // Important Note
        // This line is a must have, before switching to offboard, we must have already sent
        // a queue of positions. here we are already in a loop. so we can publish the previous location
        // what ever it is (it will hover on that specific location) until the pose is updated.
        local_pos_pub.publish(pose);

        ros::spinOnce();
        rate.sleep();

        if (land)
        {
            break;
        }
    }

    return 0;
}
```</span>
</code>```

答:
Use setpoint_velocity to turn. When you use position_setpoints then the drone will try to accelerate then decelerate causing this disturbance. By sending cmd_vel setpoints you can have a smooth turn and then you can publish your return setpoint.

Topic name.
/mavros/setpoint_velocity/cmd_vel
Message docs.


问:
Modifying Turtlebot3 to include different camera
I'm new to robotics in ROS. I've been learning about ROS and Gazebo using turtlebot (not turtlebot3 actually) on the platform The Construct. I would now like to incorporate turtlebot3 into my project.

I'm trying to build a robot which uses turtlebot3 burger as a base with the LDS (Laser Distance Sensor) removed (check out the original turtlebot3 burger spec here) and replaced with an Intel D435i camera. I would like to retain all the other aspects of the robot (dynamics, topics, nodes etc).

I have already installed the IntelRealSense/realsense-ros package from github as well as the ROBOTIS-GIT/turtlebot3 from here.
I have looked at this webpage on the Gazebo website, and it seems I need to modify the SDF model of the Turtlebot and somehow incorporate the correct links and joints to get the camera working correctly.

I don't know where to start with editing the turtlebot to include this camera.

答:
You can start editing with urdf file. Edit the file catkin_ws/src/turtlebot3/turtlebot3_description/urdf/turtlebot3_burger.gazebo.xacro.
remove the following part.

  <gazebo reference="base_scan">
    <material>Gazebo/FlatBlack</material>
    <sensor type="ray" name="lds_lfcd_sensor">
      <pose>0 0 0 0 0 0</pose>
      <visualize>$(arg laser_visual)</visualize>
      <update_rate>5</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>360</samples>
            <resolution>1</resolution>
            <min_angle>0.0</min_angle>
            <max_angle>6.28319</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.120</min>
          <max>3.5</max>
          <resolution>0.015</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_lds_lfcd_controller" filename="libgazebo_ros_laser.so">
        <topicName>scan</topicName>
        <frameName>base_scan</frameName>
      </plugin>
    </sensor>
  </gazebo>
```
<p>and add your sensor.
You will need to adjust the <code>sensor_link</code> name and also change it in the <code>joints</code>.
In the file <code>catkin_ws/src/turtlebot3/turtlebot3_description/urdf/turtlebot3_burger.urdf.xacro</code> replace the following with your appropriate sensor name.</p>
<pre class="lang-xml prettyprint-override"><code>  <joint name="scan_joint" type="fixed">
    <parent link="base_link"/>
    <child link="base_scan"/>
    <origin xyz="-0.032 0 0.172" rpy="0 0 0"/>
  </joint>

  <link name="base_scan">
    <visual>
      <origin xyz="0 0 0.0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="package://turtlebot3_description/meshes/sensors/lds.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark"/>
    </visual>

    <collision>
      <origin xyz="0.015 0 -0.0065" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.0315" radius="0.055"/>
      </geometry>
    </collision>

    <inertial>
      <mass value="0.114" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
               iyy="0.001" iyz="0.0"
               izz="0.001" />
    </inertial>
  </link>
</code>```

or use the same names in your new sensor.
For reference this is the d435 sensor plugin I use.

<?xml version="1.0" encoding="UTF-8"?>
<sdf version="1.5">
   <model name="d435">
      <pose>0 0 0 0 0 0</pose>
      <link name="base_link">
         <pose>0 0 0 0 0 0</pose>
         <inertial>
            <pose>0 0.0175 0.0125 0 -0 0</pose>
            <mass>0.564</mass>
            <inertia>
               <ixx>0.00388124</ixx>
               <ixy>0</ixy>
               <ixz>0</ixz>
               <iyy>0.00049894</iyy>
               <iyz>0</iyz>
               <izz>0.00387926</izz>
            </inertia>
         </inertial>
         <collision name="collision">
            <pose>0 0 0.0125 0 -0 0</pose>
            <geometry>
               <box>
                  <size>0.025 0.09 0.025</size>
               </box>
            </geometry>
            <surface>
               <contact>
                  <ode>
                     <kp>1e+13</kp>
                     <kd>1</kd>
                  </ode>
               </contact>
               <friction>
                  <ode>
                     <mu2>1</mu2>
                     <fdir1>0 0 0</fdir1>
                  </ode>
               </friction>
            </surface>
         </collision>
         <visual name="visual">
            <pose>0 0 0.0125 1.5708 -0 1.5708</pose>
            <geometry>
               <mesh>
                  <uri>model://d435/meshes/d435.dae</uri>
               </mesh>
            </geometry>
            <material>
               <ambient>0.5 0.5 0.5 1</ambient>
               <diffuse>0.9 0.9 0.9 0.5</diffuse>
               <specular>0.1 0.1 0.1 1</specular>
               <emissive>0 0 0 1</emissive>
               <shader type="pixel">
                  <normal_map>__default__</normal_map>
               </shader>
               <lighting>1</lighting>
            </material>
            <cast_shadows>1</cast_shadows>
         </visual>
         <sensor name="cameracolor" type="camera">
            <pose>0 0 0 0 0 0</pose>
            <camera name="camera">
               <horizontal_fov>1.500</horizontal_fov>
               <image>
                  <width>640</width>
                  <height>360</height>
                  <format>RGB_INT8</format>
               </image>
               <clip>
                  <near>0.1</near>
                  <far>30</far>
               </clip>
               <noise>
                  <type>gaussian</type>
                  <mean>0</mean>
                  <stddev>0.0</stddev>
               </noise>
            </camera>
            <always_on>1</always_on>
            <update_rate>30</update_rate>
            <visualize>0</visualize>
         </sensor>
         <sensor name="cameraired1" type="camera">
            <pose>0 0 0 0 0 0</pose>
            <camera name="camera">
               <horizontal_fov>1.500</horizontal_fov>
               <image>
                  <width>640</width>
                  <height>360</height>
                  <format>L_INT8</format>
               </image>
               <clip>
                  <near>0.1</near>
                  <far>30</far>
               </clip>
               <noise>
                  <type>gaussian</type>
                  <mean>0</mean>
                  <stddev>0.0</stddev>
               </noise>
            </camera>
            <always_on>1</always_on>
            <update_rate>30</update_rate>
            <visualize>0</visualize>
            <pose>0 0.0175 0.0125 0 -0 0</pose>
         </sensor>
         <sensor name="cameraired2" type="camera">
            <pose>0 0.05 0 0 0 0</pose>
            <camera name="camera">
               <horizontal_fov>1.500</horizontal_fov>
               <image>
                  <width>640</width>
                  <height>360</height>
                  <format>L_INT8</format>
               </image>
               <clip>
                  <near>0.1</near>
                  <far>30</far>
               </clip>
               <noise>
                  <type>gaussian</type>
                  <mean>0</mean>
                  <stddev>0.0</stddev>
               </noise>
            </camera>
            <always_on>1</always_on>
            <update_rate>30</update_rate>
            <visualize>0</visualize>
         </sensor>
         <sensor name="cameradepth" type="depth">
            <pose>0 0 0 0 0 0</pose>
            <camera name="camera">
               <horizontal_fov>1.48702</horizontal_fov>
               <image>
                  <width>640</width>
                  <height>360</height>
               </image>
               <clip>
                  <near>0.1</near>
                  <far>20</far>
               </clip>
               <noise>
                  <type>gaussian</type>
                  <mean>0</mean>
                  <stddev>0.0</stddev>
               </noise>
            </camera>
            <always_on>1</always_on>
            <update_rate>30</update_rate>
            <visualize>0</visualize>
         </sensor>
      </link>
      <plugin name="camera" filename="librealsense_gazebo_plugin.so">
         <prefix>camera</prefix>
         <depthUpdateRate>30.0</depthUpdateRate>
         <colorUpdateRate>30.0</colorUpdateRate>
         <infraredUpdateRate>30.0</infraredUpdateRate>
         <depthTopicName>depth_aligned_to_color_and_infra1/image_raw</depthTopicName>
         <depthCameraInfoTopicName>depth/camera_info</depthCameraInfoTopicName>
         <colorTopicName>color/image_raw</colorTopicName>
         <colorCameraInfoTopicName>color/camera_info</colorCameraInfoTopicName>
         <infrared1TopicName>infra1/image_raw</infrared1TopicName>
         <infrared1CameraInfoTopicName>infra1/camera_info</infrared1CameraInfoTopicName>
         <infrared2TopicName>infra2/image_raw</infrared2TopicName>
         <infrared2CameraInfoTopicName>infra2/camera_info</infrared2CameraInfoTopicName>
         <colorOpticalframeName>camera_color_optical_frame</colorOpticalframeName>
         <depthOpticalframeName>camera_depth_optical_frame</depthOpticalframeName>
         <infrared1OpticalframeName>camera_left_ir_optical_frame</infrared1OpticalframeName>
         <infrared2OpticalframeName>camera_right_ir_optical_frame</infrared2OpticalframeName>
         <rangeMinDepth>0.01</rangeMinDepth>
         <rangeMaxDepth>20.0</rangeMaxDepth>
         <pointCloud>1</pointCloud>
         <pointCloudTopicName>depth/color/points</pointCloudTopicName>
         <pointCloudCutoff>0.01</pointCloudCutoff>
         <pointCloudCutoffMax>20.0</pointCloudCutoffMax>
      </plugin>
   </model>
</sdf>
```

You can adjust the topics you need. Hope this helps.

Body:
You can start editing with urdf file. Edit the file catkin_ws/src/turtlebot3/turtlebot3_description/urdf/turtlebot3_burger.gazebo.xacro.
remove the following part.

  <gazebo reference="base_scan">
    <material>Gazebo/FlatBlack</material>
    <sensor type="ray" name="lds_lfcd_sensor">
      <pose>0 0 0 0 0 0</pose>
      <visualize>$(arg laser_visual)</visualize>
      <update_rate>5</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>360</samples>
            <resolution>1</resolution>
            <min_angle>0.0</min_angle>
            <max_angle>6.28319</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.120</min>
          <max>3.5</max>
          <resolution>0.015</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_lds_lfcd_controller" filename="libgazebo_ros_laser.so">
        <topicName>scan</topicName>
        <frameName>base_scan</frameName>
      </plugin>
    </sensor>
  </gazebo>

and add your sensor.
You will need to adjust the sensor_link name and also change it in the joints.
In the file catkin_ws/src/turtlebot3/turtlebot3_description/urdf/turtlebot3_burger.urdf.xacro replace the following with your appropriate sensor name.

  <joint name="scan_joint" type="fixed">
    <parent link="base_link"/>
    <child link="base_scan"/>
    <origin xyz="-0.032 0 0.172" rpy="0 0 0"/>
  </joint>

  <link name="base_scan">
    <visual>
      <origin xyz="0 0 0.0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="package://turtlebot3_description/meshes/sensors/lds.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark"/>
    </visual>

    <collision>
      <origin xyz="0.015 0 -0.0065" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.0315" radius="0.055"/>
      </geometry>
    </collision>

    <inertial>
      <mass value="0.114" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
               iyy="0.001" iyz="0.0"
               izz="0.001" />
    </inertial>
  </link>

or use the same names in your new sensor.
For reference this is the d435 sensor plugin I use.

<?xml version="1.0" encoding="UTF-8"?>
<sdf version="1.5">
   <model name="d435">
      <pose>0 0 0 0 0 0</pose>
      <link name="base_link">
         <pose>0 0 0 0 0 0</pose>
         <inertial>
            <pose>0 0.0175 0.0125 0 -0 0</pose>
            <mass>0.564</mass>
            <inertia>
               <ixx>0.00388124</ixx>
               <ixy>0</ixy>
               <ixz>0</ixz>
               <iyy>0.00049894</iyy>
               <iyz>0</iyz>
               <izz>0.00387926</izz>
            </inertia>
         </inertial>
         <collision name="collision">
            <pose>0 0 0.0125 0 -0 0</pose>
            <geometry>
               <box>
                  <size>0.025 0.09 0.025</size>
               </box>
            </geometry>
            <surface>
               <contact>
                  <ode>
                     <kp>1e+13</kp>
                     <kd>1</kd>
                  </ode>
               </contact>
               <friction>
                  <ode>
                     <mu2>1</mu2>
                     <fdir1>0 0 0</fdir1>
                  </ode>
               </friction>
            </surface>
         </collision>
         <visual name="visual">
            <pose>0 0 0.0125 1.5708 -0 1.5708</pose>
            <geometry>
               <mesh>
                  <uri>model://d435/meshes/d435.dae</uri>
               </mesh>
            </geometry>
            <material>
               <ambient>0.5 0.5 0.5 1</ambient>
               <diffuse>0.9 0.9 0.9 0.5</diffuse>
               <specular>0.1 0.1 0.1 1</specular>
               <emissive>0 0 0 1</emissive>
               <shader type="pixel">
                  <normal_map>__default__</normal_map>
               </shader>
               <lighting>1</lighting>
            </material>
            <cast_shadows>1</cast_shadows>
         </visual>
         <sensor name="cameracolor" type="camera">
            <pose>0 0 0 0 0 0</pose>
            <camera name="camera">
               <horizontal_fov>1.500</horizontal_fov>
               <image>
                  <width>640</width>
                  <height>360</height>
                  <format>RGB_INT8</format>
               </image>
               <clip>
                  <near>0.1</near>
                  <far>30</far>
               </clip>
               <noise>
                  <type>gaussian</type>
                  <mean>0</mean>
                  <stddev>0.0</stddev>
               </noise>
            </camera>
            <always_on>1</always_on>
            <update_rate>30</update_rate>
            <visualize>0</visualize>
         </sensor>
         <sensor name="cameraired1" type="camera">
            <pose>0 0 0 0 0 0</pose>
            <camera name="camera">
               <horizontal_fov>1.500</horizontal_fov>
               <image>
                  <width>640</width>
                  <height>360</height>
                  <format>L_INT8</format>
               </image>
               <clip>
                  <near>0.1</near>
                  <far>30</far>
               </clip>
               <noise>
                  <type>gaussian</type>
                  <mean>0</mean>
                  <stddev>0.0</stddev>
               </noise>
            </camera>
            <always_on>1</always_on>
            <update_rate>30</update_rate>
            <visualize>0</visualize>
            <pose>0 0.0175 0.0125 0 -0 0</pose>
         </sensor>
         <sensor name="cameraired2" type="camera">
            <pose>0 0.05 0 0 0 0</pose>
            <camera name="camera">
               <horizontal_fov>1.500</horizontal_fov>
               <image>
                  <width>640</width>
                  <height>360</height>
                  <format>L_INT8</format>
               </image>
               <clip>
                  <near>0.1</near>
                  <far>30</far>
               </clip>
               <noise>
                  <type>gaussian</type>
                  <mean>0</mean>
                  <stddev>0.0</stddev>
               </noise>
            </camera>
            <always_on>1</always_on>
            <update_rate>30</update_rate>
            <visualize>0</visualize>
         </sensor>
         <sensor name="cameradepth" type="depth">
            <pose>0 0 0 0 0 0</pose>
            <camera name="camera">
               <horizontal_fov>1.48702</horizontal_fov>
               <image>
                  <width>640</width>
                  <height>360</height>
               </image>
               <clip>
                  <near>0.1</near>
                  <far>20</far>
               </clip>
               <noise>
                  <type>gaussian</type>
                  <mean>0</mean>
                  <stddev>0.0</stddev>
               </noise>
            </camera>
            <always_on>1</always_on>
            <update_rate>30</update_rate>
            <visualize>0</visualize>
         </sensor>
      </link>
      <plugin name="camera" filename="librealsense_gazebo_plugin.so">
         <prefix>camera</prefix>
         <depthUpdateRate>30.0</depthUpdateRate>
         <colorUpdateRate>30.0</colorUpdateRate>
         <infraredUpdateRate>30.0</infraredUpdateRate>
         <depthTopicName>depth_aligned_to_color_and_infra1/image_raw</depthTopicName>
         <depthCameraInfoTopicName>depth/camera_info</depthCameraInfoTopicName>
         <colorTopicName>color/image_raw</colorTopicName>
         <colorCameraInfoTopicName>color/camera_info</colorCameraInfoTopicName>
         <infrared1TopicName>infra1/image_raw</infrared1TopicName>
         <infrared1CameraInfoTopicName>infra1/camera_info</infrared1CameraInfoTopicName>
         <infrared2TopicName>infra2/image_raw</infrared2TopicName>
         <infrared2CameraInfoTopicName>infra2/camera_info</infrared2CameraInfoTopicName>
         <colorOpticalframeName>camera_color_optical_frame</colorOpticalframeName>
         <depthOpticalframeName>camera_depth_optical_frame</depthOpticalframeName>
         <infrared1OpticalframeName>camera_left_ir_optical_frame</infrared1OpticalframeName>
         <infrared2OpticalframeName>camera_right_ir_optical_frame</infrared2OpticalframeName>
         <rangeMinDepth>0.01</rangeMinDepth>
         <rangeMaxDepth>20.0</rangeMaxDepth>
         <pointCloud>1</pointCloud>
         <pointCloudTopicName>depth/color/points</pointCloudTopicName>
         <pointCloudCutoff>0.01</pointCloudCutoff>
         <pointCloudCutoffMax>20.0</pointCloudCutoffMax>
      </plugin>
   </model>
</sdf>

You can adjust the topics you need. Hope this helps.


问:
Proper implementation of `pure_pursuit` for ground robots using waypoints
I am trying to understand and implement pure-pursuit waypoint following in 2D map. My questions are:

1. What exactly do I need to record in my waypoint.csv file?

To get some idea, I was looking at this waypoint_logger example where a single waypoint is defined in the form of

```<code>{  data.pose.pose.position.x,
   data.pose.pose.position.y,
   euler[2],
   speed  }
</code>```

where the first two components are 2D pose from robot odometry, the third component includes 3rd euler component from euler_from_quaternion conversion of odometry orientation and last one represents the velocity (from odometry twist component).

2. How to implement pure_pursuit based on recorded waypoints?

Again looking at examples, this implementation (if I understood correctly) requires waypoints to be in the form of

```<code>{   data.pose.pose.position.x
    data.pose.pose.position.y
    data.pose.pose.orientation.z
    data.pose.pose.orientation.w  }
</code>```

Another implementation of pure_pursuit requires nav_msgs::Odometry odom and nav_msgs::Path path from the robot in order to construct ackermann_msgs. Neither use velocity/speed, instead need other information.

Any reference/guide is appreciated.

答:
Is pure pursuit something like this?

Here my agent is chasing a moving target that has 60-70% of its velocity. The agent is given only the information of the present position of the robot. So if you are pursuing a string of recorded waypoints, maybe my method can help. At every instant, the robot looks for its heading correction. If the heading is within a threshold, then nothing happens. Otherwise, the agent corrects itself and proceeds along that heading till it reaches an endpoint. So the controller looks something like this.

You can publish your recorded waypoints to a topic that gets updated at an interval then your agent can pursue that target until it gets a new target and pursues it. You can define a target threshold that whenever the agent reaches within the target position, it stops. You will not need velocity and yaw for this. You will just be needing the position setpoints.

Here's the code for the above implementation.

#!/usr/bin/env python

# This Node Controls and Navigates the robot to a given goal position.

# import needed libraries
import rospy
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from std_msgs.msg import Float64
import matplotlib.pyplot as plt
import numpy as np
from tf.transformations import euler_from_quaternion
import math
import time

# define some variables
pose = [0] * 3
yaw = 0
gx = 10
gy = 0

def goalx_callback(data):
    global gx
    gx = data.data

def goaly_callback(data):
    global gy
    gy = data.data

def odom_callback(data):
    global pose, yaw

    x = data.pose.pose.orientation.x
    y = data.pose.pose.orientation.y
    z = data.pose.pose.orientation.z
    w = data.pose.pose.orientation.w
    pose = [data.pose.pose.position.x, data.pose.pose.position.y,
            euler_from_quaternion([x, y, z, w])[2]]

    yaw = pose[2]

# main Control Loop
def control_loop():
    # initialize node name
    rospy.init_node('rover1_controller')
    # initialize velocity publisher
    pub = rospy.Publisher('rover1/cmd_vel', Twist, queue_size=10)
    global pose, yaw
    global gx, gy
    # subscribe to topics required(odometry and goal positions)
    rospy.Subscriber('rover1/odom', Odometry, odom_callback)
    rospy.Subscriber('/goal_x', Float64, goalx_callback)
    rospy.Subscriber('/goal_y', Float64, goaly_callback)
    # define ros rate
    rate = rospy.Rate(10)
    # define velocity msg and publish a 0 initially
    velocity_msg = Twist()
    velocity_msg.linear.x = 0
    velocity_msg.angular.z = 0

    pub.publish(velocity_msg)

    # Loop which runs until target is reached

    while not rospy.is_shutdown():
        # Present Coordinates
        present_point = [pose[0], pose[1]]
        y = gy - pose[1]
        x = gx - pose[0]
        # angle at which robot should move
        steering_angle = math.atan2(y, x)

        angle = steering_angle - yaw
        print(gx, gy)
        # calculate required yaw
        yaw_precision = (math.pi / 180) * 2  # +- 2 degree allowed

        yaw_error = math.fabs(angle) - yaw_precision
        # define angular velocity
        if angle != 0:
            angular_vel = 1 * math.fabs(angle) / angle
        else:
            angular_vel = 1

        # define euclidean distance between robot and target
        dist = math.sqrt(math.pow(pose[1] - gy, 2) +
                         (math.pow(pose[0] - gx, 2)))

        dist_precision = 0.1
        dist_error = dist - dist_precision

        # control loop
        # adjust yaw if needed
        if yaw_error > 0:

            # angle adjustment using yaw
            velocity_msg.angular.x = 0
            velocity_msg.angular.y = 0
            velocity_msg.angular.z = angular_vel

            pub.publish(velocity_msg)

        elif yaw_error <= 0:

            velocity_msg.angular.x = 0
            velocity_msg.angular.y = 0
            velocity_msg.angular.z = 0

            pub.publish(velocity_msg)

            # adjust distance after yaw
            # break the loop if the distance between robot and target is less than 1
            if dist_error > 1:

                velocity_msg.linear.x = 1
                pub.publish(velocity_msg)

            elif dist_error <= 1:

                velocity_msg.linear.x = 0
                pub.publish(velocity_msg)
                break
        rate.sleep()

    # send a 0 vel to stop the robot
    velocity_msg.linear.x = 0
    velocity_msg.angular.z = 0

    pub.publish(velocity_msg)

# Call the controller function
if __name__ == '__main__':
    try:
        print('start')
        control_loop()
    except rospy.ROSInterruptException:
        pass
```

Hope this helps.

Body:
Is pure pursuit something like this?

Here my agent is chasing a moving target that has 60-70% of its velocity. The agent is given only the information of the present position of the robot. So if you are pursuing a string of recorded waypoints, maybe my method can help. At every instant, the robot looks for its heading correction. If the heading is within a threshold, then nothing happens. Otherwise, the agent corrects itself and proceeds along that heading till it reaches an endpoint. So the controller looks something like this.

You can publish your recorded waypoints to a topic that gets updated at an interval then your agent can pursue that target until it gets a new target and pursues it. You can define a target threshold that whenever the agent reaches within the target position, it stops. You will not need velocity and yaw for this. You will just be needing the position setpoints.

Here's the code for the above implementation.

#!/usr/bin/env python

# This Node Controls and Navigates the robot to a given goal position.

# import needed libraries
import rospy
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from std_msgs.msg import Float64
import matplotlib.pyplot as plt
import numpy as np
from tf.transformations import euler_from_quaternion
import math
import time

# define some variables
pose = [0] * 3
yaw = 0
gx = 10
gy = 0

def goalx_callback(data):
    global gx
    gx = data.data

def goaly_callback(data):
    global gy
    gy = data.data

def odom_callback(data):
    global pose, yaw

    x = data.pose.pose.orientation.x
    y = data.pose.pose.orientation.y
    z = data.pose.pose.orientation.z
    w = data.pose.pose.orientation.w
    pose = [data.pose.pose.position.x, data.pose.pose.position.y,
            euler_from_quaternion([x, y, z, w])[2]]

    yaw = pose[2]

# main Control Loop
def control_loop():
    # initialize node name
    rospy.init_node('rover1_controller')
    # initialize velocity publisher
    pub = rospy.Publisher('rover1/cmd_vel', Twist, queue_size=10)
    global pose, yaw
    global gx, gy
    # subscribe to topics required(odometry and goal positions)
    rospy.Subscriber('rover1/odom', Odometry, odom_callback)
    rospy.Subscriber('/goal_x', Float64, goalx_callback)
    rospy.Subscriber('/goal_y', Float64, goaly_callback)
    # define ros rate
    rate = rospy.Rate(10)
    # define velocity msg and publish a 0 initially
    velocity_msg = Twist()
    velocity_msg.linear.x = 0
    velocity_msg.angular.z = 0

    pub.publish(velocity_msg)

    # Loop which runs until target is reached

    while not rospy.is_shutdown():
        # Present Coordinates
        present_point = [pose[0], pose[1]]
        y = gy - pose[1]
        x = gx - pose[0]
        # angle at which robot should move
        steering_angle = math.atan2(y, x)

        angle = steering_angle - yaw
        print(gx, gy)
        # calculate required yaw
        yaw_precision = (math.pi / 180) * 2  # +- 2 degree allowed

        yaw_error = math.fabs(angle) - yaw_precision
        # define angular velocity
        if angle != 0:
            angular_vel = 1 * math.fabs(angle) / angle
        else:
            angular_vel = 1

        # define euclidean distance between robot and target
        dist = math.sqrt(math.pow(pose[1] - gy, 2) +
                         (math.pow(pose[0] - gx, 2)))

        dist_precision = 0.1
        dist_error = dist - dist_precision

        # control loop
        # adjust yaw if needed
        if yaw_error > 0:

            # angle adjustment using yaw
            velocity_msg.angular.x = 0
            velocity_msg.angular.y = 0
            velocity_msg.angular.z = angular_vel

            pub.publish(velocity_msg)

        elif yaw_error <= 0:

            velocity_msg.angular.x = 0
            velocity_msg.angular.y = 0
            velocity_msg.angular.z = 0

            pub.publish(velocity_msg)

            # adjust distance after yaw
            # break the loop if the distance between robot and target is less than 1
            if dist_error > 1:

                velocity_msg.linear.x = 1
                pub.publish(velocity_msg)

            elif dist_error <= 1:

                velocity_msg.linear.x = 0
                pub.publish(velocity_msg)
                break
        rate.sleep()

    # send a 0 vel to stop the robot
    velocity_msg.linear.x = 0
    velocity_msg.angular.z = 0

    pub.publish(velocity_msg)

# Call the controller function
if __name__ == '__main__':
    try:
        print('start')
        control_loop()
    except rospy.ROSInterruptException:
        pass

Hope this helps.


问:
Connect LiPo battery to Arduino Motor Driver Shield for RC Boat
Thanks for taking the time reading this question! It is greatly appreciated.

What are we doing

We (two complete noobs) are building a remote controlled boat from scratch using Arduino compatible components. With a hull created of paper-foarm soaked in glue and expoxy we aim to create a sturdy vessel capable of carrying a can of beer at a reasonable speed across the local waters.

What are we trying to use

Using an Arduino UNO for the boat, Arduino Nano for the remote control (housed in a modified PS4 controller body) and a bunch of sensors, OLED screens and NRF24L01 transceivers we have gotten the basics and some fancy extra's sorted.

However we are unsure about our most crucial components. After some failed attempts we have discovered we need a motor with enough torque or/and a gearbox to be able to propel our vessel within actual water. These higher specced motors also require a beefier power source and thus we've been looking into LiPo-batteries. This is where we got stuck!

Our research has brought us to selecting the following components:

2x RS PRO Geared DC Motor, 7.98 W, 6 → 15 V, 78.4 gcm, 9869 rpm, ~1A
operating, 5A Stall current @
https://docs.rs-online.com/216e/A700000007670040.pdf

Cytron 10A 7-30V Dual Channel DC Motor Driver Shield MDD10 @
https://www.robotshop.com/media/files/content/c/cyt/pdf/cytron-10a-7-30v-dual-channel-dc-motor-driver-shield-datasheet.pdf

Gens Ace Soaring 2200mAh 14.8V 30C 4S1P Lipo battery - XT60 plug

2x ACS712 ACS712ELC Current Sensor Module 5A

Avio Max Pro 04 20W Lipo charger

Specific questions

Primary:

How do we connect the LiPo battery to the Motor Driver Shield in a safe manner?

Secondary:

Can the LiPo be connected using a XT60 to Dupont cable?

Is the motor torque of 78.4 gcm / 784 ncm at 9869 rpm sufficient for water movement?

Do we even need the current sensor modules?

Is the charger correct for this battery?

Any other recommended H-bridge or Motor Driver that supports near 10A?

Tertiary:

Shoot any holes into our plan - any and all suggestions welcome!

答: You connect a battery to the shield via the power input terminal as described in the data sheet you linked. Refer to figures 1 and 4, and table 1.</p>


答:
Reducing reflection on an image is hard, really hard, because you only have the pixels, and they are most likely white.
So changing the environment to get good images is the desired solution.
Robotiq wrote a nice article about it:
https://blog.robotiq.com/10-solutions-to-improve-robot-vision-with-shiny-objects

You can however detect reflection using OpenCV, and then decide what to do, for example:

fill the reflection with the average color of the pixels around

Ignore the reflection area.

If your reflection is not covering everything you might want to look into deep learning instead of openCV, i found it much easier to cover all cases using a good model, than to create filters in openCV that covered every scenario.

note that I don't have any experience related to reflection regarding the realsense or Lidar.

Title: Reflective surface

Body: Have you encountered the problem of reflective surfaces for realsense camera (d435 or t265) and lidar (A1 or A2). I would like to know what are the ways to solve problems with reflective surfaces for them programmatically, without using other sensors such as sonars. Perhaps there are any solutions at OpenCV. I would be very glad to hear people who tried to solve a similar problem. Thank you for attention</p>


问:
Number of diagonal elements in the pose covariance matrix
The control.yaml file inside the husky_gazebo package defines the value of the  pose_covariance_diagonal parameter as  [0.001, 0.001, 0.001, 0.001, 0.001, 0.03].

A pose is made up of 7 elements - [x, y, z,q_x, q_y, q_z, q_w]. So, shouldn't the number of elements in the pose_covariance_diagonal parameter  be 7?

答:
The quaternion part [q_x, q_y, q_z, q_w] has four numbers but is a representation of 3D orientation, which has 3 degrees of freedom. Another common representation for orientation is the matrix Lie group $\mathrm{SO}(3)$, which is the group of $3\times 3$ rotation matrices (9 numbers, but only 3 degrees of freedom). Neither the quaternion nor the rotation matrix are minimal representations (i.e., they take more numbers than the degrees of freedom), but they do not suffer the issues associated with, e.g., Euler angles (gimbal lock, ambiguity, etc.).

So, there are only 6 uncertainty values in pose_covariance_diagonal because there are only 6 degrees of freedom in a 3D pose (i.e., the matrix Lie group $\mathrm{SE}(3)$). These are uncertainties for each translation axis, and each rotation axis. See the PoseWithCovariance msg docs for more info.


问:
Sensor fusion with gyroscope and motor rotations
I am building a robot (2 powered wheels and one ball bearing). The problem is that I can't seem to make it drive straight. I literally find it impossible, I have been trying for weeks.

I have two gyro sensors (on each wheel) and the current motor rotations.

The end result would be to have the gyros at 0 degrees, I do not really care for motor rotations.

I tried to search on Google but most results are GPS related. Is there a way I can do this?

答:
You need to describe your system as a linear system, preferably with a matrix notation.
then change the base or make the state variables exactly what you want to measure, finally with an ¿observer' you would estimate any state if it is observable even if you are not measuring it directly, if it could be represented as a linear combination of its states then it will be observable.

Using two giroscopies in a rigid body actually does'nt help too much, just reduce a little the radius of the gaussian noise (the variance of normal distribution is the mean of the variances, probably reduce noise in half). It could be different if they are both rotating since the correlation will be broken and the information provided by them won't be the same/similar.

But you probably don't need two giroscopies if they are attached to the rigid body...
To achieve what you want neither need sensor fusion, you only need to describe the system with inputs motor velocities and  output as the angle of the rigid body, then close the loop using one gyroscope in order to feedback the error.


答:
Adding to Parker's answer, the quaternion is avoided many cases due to its complexity in getting a closed-form Jacobian. Also, it unnecessarily increases the number of optimization states. Due to these problems rotation vector (3x1) is usually preferred and that's why you see it as 3x1. Note that the rotation vector is different from Euler angles and does not have the gimbal lock and ambiguity problem.

Also, in terms of rotation uncertainty representation, the rotation vector is more intuitive than quaternion.

See "state estimation book" by Tim Barfoot for more information.

Title: Number of diagonal elements in the pose covariance matrix

Body:
The control.yaml file inside the husky_gazebo package defines the value of the  pose_covariance_diagonal parameter as  [0.001, 0.001, 0.001, 0.001, 0.001, 0.03].

A pose is made up of 7 elements - [x, y, z,q_x, q_y, q_z, q_w]. So, shouldn't the number of elements in the pose_covariance_diagonal parameter  be 7?


问:
How to know the desired orientation of a quadcopter?
I am trying to simulate a quadcopter model on Simulink. I want to implement a PID controller for each of X,Y,Z and phi,theta, psi angles. PID gets the error, as input, which is to be minimized.
For the X,Y and Z, the desired values are entered by the user and the actual values are calculated from the accelerometer data, hence, the error is the desired set value - actual value.

For phi,theta and psi, the actual values may be obtained from the gyroscope and accelerometer (sensor fusion) but I don't actually know how to calculate the desired values for each one of them since the user is usually interested in giving the position values X,Y and Z as desired not the angle values! The absence of the desired values prevents me form calculating the angular error which is needed for the PID controller.

答:
You're trying to implement more PIDs than you have degrees of freedom.  In a quadcopter, you have only 4: $(Z, \phi, \theta, \psi)$ i.e. (Altitude, Roll, Pitch, and Yaw).

via (http://www.draganfly.com/uav-helicopter/draganflyer-x4/features/stability.php)

Interestingly, from a PID perspective you definitely do have desired values for $\phi$ and $\theta$: you want them both to default to zero, so that your quadcopter hovers stably.  (You could also constrain $\psi$ to a fixed value like zero, but since the quadcopter is symmetric about the Z axis, all you really need to do is prevent excessive rotation by bumping up the 'D' term in the $\psi$ PID.)  The Z PID can then be tuned to ensure that you can set and hold a desired altitude.

So in the simplest case, the X and Y PIDs will end up sitting "on top" of the other PIDs.

In other words, the output of the X and Y PIDs will be (respectively) become the desired $\phi$ and $\theta$ angles that will nudge the quadcopter toward the desired position.  (Note that you'll have to convert the world-relative desired X and Y to quadcopter-relative X and Y to compensate for $\psi$.  This answer has more.)

There are more sophisticated ways to control aerial vehicles, but this is one of the easiest to learn.


问:
Using Google Cartographer library standalone
I am working on a project that requires the analysis of the results of different SLAM libraries with respect their accuracy on building a map. Right now, I am more interested in the development with these libraries, and one of the requirements is to NOT using ROS (we have our own framework)

So far, I have implemented the code using Karto, and my next step is implementing it with Google Cartographer, so I first looked for the docs, tutorials or examples of using standalone Cartographer library without success.. Hence, here I am: do you know or can share a minimum project or guide to use Cartographer library?

I have "translated" the ROS node, and reached a point in which I have a (not) working example: it creates an almost empty map :'(. I am using Odometry and Lidar data to fill in the trajectory, and the result is a map with some sparse gray pixels (I am using a PGM writer).

My code is more or less like the following

```<code>float resolution = 0.05;

mapping::ProbabilityGridRangeDataInserter2D range_data_inserter(range_data_inserter_options);
auto grid_map = io::CreateProbabilityGrid(resolution, &conversion_tables);
auto map_builder = mapping::CreateMapBuilder(opts_map_builder);

SensorId odom_sensor;
odom_sensor.type = TrajectoryBuilderInterface::SensorId::SensorType::ODOMETRY;
odom_sensor.id = "odom";

SensorId lidar_sensor;
lidar_sensor.type = TrajectoryBuilderInterface::SensorId::SensorType::RANGE;
lidar_sensor.id = "lidar";

auto trajectory_id = map_builder->AddTrajectoryBuilder({odom_sensor, lidar_sensor}, opts_trajectory, &LocalSLAMResultCb);

auto trajectory_builder = map_builder->GetTrajectoryBuilder(trajectory_id);

sub_odom.registerCallback(&OnOdometryMessage);
sub_lidar.registerCallback(&OnLidarMessage);

....

map_builder->FinishTrajectory(trajectory_id);
map_builder->pose_graph()->RunFinalOptimization();
</code>```

The callbacks for the odometry and lidar are the following:

odometry:

```<code>sensor::OdometryData data{FromProtoTimestamp(odom.stamp()),
                                  transform::ToRigid3(protoOdom.odometry_data().pose())};
trajectory_builder->AddSensorData(odom_sensor.id, data);
</code>```

lidar:

```<code>sensor::TimedPointCloud ranges;
auto points_batch = ::absl::make_unique<io::PointsBatch>();

for (const auto &point : lidar.cloud()) {
   sensor::TimedRangefinderPoint p;
   p.position << cosf(point.angle()) * point.distance(), sinf(point.angle()) * point.distance(), 0.0f;
   p.time = 0.0f;
   points_batch->points.push_back({p.position});
   ranges.push_back(std::move(p));
}

sensor::TimedPointCloudData point_cloud{
   FromProtoTimestamp(lidar.stamp()),
   {lidar.pose().x(), lidar.pose().y(), 0},
   ranges
};

trajectory_builder->AddSensorData(sensor.id, point_cloud);
points_batch->origin << lidar.pose().x(), lidar.pose().y(), 0;

range_data_inserter->Insert({points_batch->origin, sensor::PointCloud(points_batch->points), {}}, grid);
</code>```

I feel that I am missing a transformation, or the data I am providing is not well transformed.

When I use Karto, the result is something like this:

Hope you can help me, or if you know or can share some examples would be great :)

答:
Ok, so to whoever wants to know or has a similar issue, the problem was, indeed, related to the transformation between the global frame and the point cloud frame.

The lidar message I have contains only distance and angle, without intensities. So, when I create a TimedRangefinderPoint I transform the distance and angle into a point in the world reference frame. Hence:

```<code>p.position << odom_x + cosf(point.angle()) * point.distance(), odom_y + sinf(point.angle()) * point.distance(), 0.0f;
</code>```

being odom_x and odom_y the odometry estimation in x and y.

I will clean a bit the code, and I will post some kind of minimum tutorial to make this library to work.

Edit
Not a tutorial, but an example of usage of the standalone library: https://github.com/adiego73/cartographer If you think there are things wrong or can be changed for a better code, let me know! :)

Body:
Ok, so to whoever wants to know or has a similar issue, the problem was, indeed, related to the transformation between the global frame and the point cloud frame.

The lidar message I have contains only distance and angle, without intensities. So, when I create a TimedRangefinderPoint I transform the distance and angle into a point in the world reference frame. Hence:

p.position << odom_x + cosf(point.angle()) * point.distance(), odom_y + sinf(point.angle()) * point.distance(), 0.0f;

being odom_x and odom_y the odometry estimation in x and y.

I will clean a bit the code, and I will post some kind of minimum tutorial to make this library to work.

Edit
Not a tutorial, but an example of usage of the standalone library: https://github.com/adiego73/cartographer If you think there are things wrong or can be changed for a better code, let me know! :)


问:
Line Breaks EPSON RC+
Maybe this question belongs on StackOverflow, but it's a robotics software specific question so I wasn't sure.

Is there any way to cut a super long line in two in EPSON RC+ 7.4.8? The SPEL + syntax is pretty similar to VBA where you can use " _ " to stop a line and continue it on the next line, but I cannot find mention of any such character in any of the manuals.

答: To the best of my knowledge for the maximum line length for SPEL is 512 characters and there is no line continue character.</p>

Title: Line Breaks EPSON RC+


问:
Bug in code of Self-Balancing Bot
I am making a Self balancing bot as my project in which I am using MPU6050, Arduino Uno, motor driver l298n and DC motors.
I am making this using data from accelerometer and gyroscope and combining them through complementary filter, after that using PID and error of angle made by bot to get the PWM required for DC motor.
1) The values of PID and angles resultant of complementary filter drifts to the extreme every time I run this which I guess shouldn't happen but the values of accelerometer and gyro(without using filter) gives me approx values , I guess this is due to my mistake in the calculation of angles or in the filter that I have applied which I have been trying to but can't find it.

2) I have tried to calibrate MPU6050 many times but failed as the x-axis value is showing 0 only when I tilt the bot to 12-14 degrees on x-axis(data from only accelometer),(if you can just tell me the variable to modify).

```<code>#include <Wire.h>
long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ,elapsedTime;
long gyroX, gyroY, gyroZ;
float rotX, rotY, rotZ;
double x,y,z,dx,dy,dz,ex,ey,ez,desx=0,desy=0,desz=0;
float PID, PWM, error, previous_error,timePrev,time1;
float pid_p;
float pid_i;
float pid_d;
float rad_to_deg = 180/3.141592654;
/////////////////PID CONSTANTS/////////////////
double kp=2;
double ki=0;
double kd=0;
int motor1pin1 = 2;
int motor1pin2 = 3;
int led = 9;           // the PWM pin the LED is attached to
int brightness = 0;    // how bright the LED is
int fadeAmount = 5;

void setup()
{
  Serial.begin(115200);
  Wire.begin();
  pinMode(motor1pin1, OUTPUT);
  pinMode(motor1pin2, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(13, OUTPUT);
  setupMPU();
  delay(500);
}

void loop()
{
    timePrev = time1;  // the previous time is stored before the actual time read
    time1 = millis();  // actual time read/
    elapsedTime = (time1 - timePrev) / 1000;
    recordAccelRegisters();
    recordGyroRegisters();
   processangle();
    Apply_PID();
    printData();
    delay(100);
}
void right(int PWM)
{
   analogWrite(9, PWM);
  digitalWrite(motor1pin1, HIGH);
  digitalWrite(motor1pin2, LOW);
  digitalWrite(13, HIGH);
  Serial.println("RIGHT");
}
void left(int PWM)
{
   analogWrite(9, PWM);
  digitalWrite(motor1pin1, LOW);
  digitalWrite(motor1pin2, HIGH);
   Serial.println("LEFT");
}
void stopper()
{
   analogWrite(9, PWM);
  digitalWrite(motor1pin1, LOW);
  digitalWrite(motor1pin2, HIGH);
   Serial.println("STOPPED");
  }
void setupMPU()
{
  Wire.beginTransmission(0b1101000); //This is the I2C address of the MPU (b1101000/b1101001 for AC0 low/high datasheet sec. 9.2)
  Wire.write(0x6B); //Accessing the register 6B - Power Management (Sec. 4.28)
  Wire.write(0b00000000); //Setting SLEEP register to 0. (Required; see Note on p. 9)
  Wire.endTransmission();
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x1B); //Accessing the register 1B - Gyroscope Configuration (Sec. 4.4)
  Wire.write(0x00000000); //Setting the gyro to full scale +/- 250deg./s
  Wire.endTransmission();
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x1C); //Accessing the register 1C - Acccelerometer Configuration (Sec. 4.5)
  Wire.write(0b00000000); //Setting the accel to +/- 2g
  Wire.endTransmission();
}

void recordAccelRegisters()
{
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x3B); //Starting register for Accel Readings
  Wire.endTransmission();
  Wire.requestFrom(0b1101000,6); //Request Accel Registers (3B - 40)
  while(Wire.available() < 6);
  accelX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
  accelY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
  accelZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
  processAccelData();
}

void processAccelData()
{
  gForceX = accelX / 16384.0;
  gForceY = accelY / 16384.0;
  gForceZ = accelZ / 16384.0;
  x= (RAD_TO_DEG * (atan2(-gForceY, -gForceZ)+PI));
  y= RAD_TO_DEG * (atan2(-gForceX, -gForceZ)+PI);
  z= RAD_TO_DEG * (atan(sqrt(square(-gForceY) + square(-gForceX)) / -gForceZ));
 /* x=map(x,340,11,-90,90);*/

    Serial.print(" AccelX=");
  Serial.print(x);

}

void recordGyroRegisters()
{
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x43); //Starting register for Gyro Readings
  Wire.endTransmission();
  Wire.requestFrom(0b1101000,6); //Request Gyro Registers (43 - 48)
  while(Wire.available() < 6);
  gyroX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
  gyroY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
  gyroZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
  processGyroData();
}

void processGyroData()
{
  rotX = (gyroX / 131.0);
  rotY = gyroY / 131.0;
  rotZ = gyroZ / 131.0;
  Serial.print(" GyroX=");
  Serial.println(rotX*elapsedTime);
}
void processangle()
{
  dx=0.98 *(dx + rotX*elapsedTime) + 0.02*x;
  dy=0.98 *(dy + rotY*elapsedTime) + 0.02*y;
  dz=0.98 *(dz + rotZ*elapsedTime) + 0.02*z;
  ex=dx-desx;
  ey=dy-desy;
  ez=dz-desz;
  Serial.print("After Filter Error");
  Serial.println(ex);
}
void Apply_PID()
{
  float l=(float)ex;
  pid_p=kp*l;
  pid_i = pid_i+(ki*l);
pid_d = kd *((l - previous_error)/elapsedTime);
PID = pid_p + pid_i + pid_d;
/*Serial.print("I=");
Serial.println(pid_d);
Serial.print("p=");
Serial.println(pid_p);*/
Serial.print("PID");
Serial.println(PID);

if(l>5.5 || l<5.5)
  {
    if(l<0)
    right(PID);
    else
    left(PID);
  }
  else
  {
    stopper();
    }
previous_error = l;
}

void printData()
{
  /*Serial.print("Gyro (deg)");
  Serial.print(" X=");
  Serial.print(rotX);
  Serial.print(" Y=");
  Serial.print(rotY);
  Serial.print(" Z=");
  Serial.print(rotZ);
  Serial.print(" Accel (g)");
  Serial.print(" X=");
  Serial.print(gForceX);
  Serial.print(" Y=");
  Serial.print(gForceY);
  Serial.print(" Z=");
  Serial.println(gForceZ);*/
 /* Serial.print(" PWM");
  Serial.println(PID);*/
}
</code>```

答:
There's a lot going on here, and the issues aren't very clear to me. I would suggest trying to tackle your problems one at a time, and to not move on until you are confident the root issues are sorted out. For example, you have trouble with your PID output, but you also have issues with your angle estimates. You'll never fix the PID controller until you get your estimates correct. But you're also having issues with your calibration, in what sounds like maybe a data processing or register reading issue, and again you'll never get your estimates correct until you get your data handling fixed.

I would urge anyone working on a project to use as many existing libraries as possible - don't spend your time reinventing the wheel unless that is what your project is supposed to do. Your question here is titled, "Bug in code of self-balancing bot," so I would suggest you use the existing libraries for the IMU, use a library for processing the IMU, and then that frees you to focus solely on the self-balancing robot.

There is an MPU6050 library at this Adafruit tutorial, and I always highly recommend the Madgwick filter for converting IMU outputs to pose estimates. The page I linked for the Madgwick filter has implementations in C, C#, and Matlab.

That said, I can't tell what exactly the problems are that you're facing. When you say

the x-axis value is showing 0 only when I tilt the bot to 12-14 degrees on x-axis(data from only accelometer)

I don't know what that means. It looks like you're trying to estimate angles from the accelerometers and trying to filter angular rates from the gyros. Are you meaning the angle about x is zero? The x gyro is zero? The x accelerometer is zero? I can't tell. Which way do you have the IMU oriented? When you say you're tilting the bot on x-axis, do you mean you're turning it about the x-axis? About some other axis?

Please give the MPU6050 library a try and see if that at least fixes your x-axis value showing zero. If you can get that working for you then maybe you can keep on with your complimentary filter and can get the rest of your code sorted out.

Once you get the readings coming in as expected, if your filtering doesn't work out, give the Madgwick filter a shot. That should give you very, very stable and accurate orientation estimates but, as I mentioned at the top of this, it'll never work unless you're able to get the readings off the chip correctly.

If you'd like more support with leaving all of your code as-is, please edit your question to provide more information like how your IMU is oriented, how you're turning it, a link to the datasheet you're using, etc.

Title: Bug in code of Self-Balancing Bot