问:
What is map_load_time in ROS nav_msgs/MapMetaData?
http://docs.ros.org/api/nav_msgs/html/msg/MapMetaData.html

The description says it is 'The time at which the map was loaded'- should this be the time the map was initialized? The time it was last updated (making it redundant with the header timestamp)?

PS: I am asking the same question at https://answers.ros.org/question/114726/what-is-map_load_time-in-nav_msgsmapmetadata/ that nobody has answered so far.

答:
I have a lot of ros packages checked out into a big catkin workspace, looks like it is redundant with the header stamp:

```<code>$ git grep --no-index -n -C 2 --heading map_load_time
other/jsk_common/jsk_ros_patch/multi_map_server/src/main.cpp
155-      ROS_INFO("Loading map from image \"%s\"", mapfname.c_str());
156-      map_server::loadMapFromFile(&map_resp_,mapfname.c_str(),res,negate,occ_th,free_th, origin);
157:      map_resp_.map.info.map_load_time = ros::Time::now();
158-      map_resp_.map.header.frame_id = frame_id;
159-      map_resp_.map.header.stamp = ros::Time::now();
--
ros/grid_map/grid_map_ros/src/GridMapRosConverter.cpp
275-  occupancyGrid.header.frame_id = gridMap.getFrameId();
276-  occupancyGrid.header.stamp.fromNSec(gridMap.getTimestamp());
277:  occupancyGrid.info.map_load_time = occupancyGrid.header.stamp;  // Same as header stamp as we do not load the map.
278-  occupancyGrid.info.resolution = gridMap.getResolution();
279-  occupancyGrid.info.width = gridMap.getSize()(0);
--
ros/navigation/map_server/src/main.cpp
154-      // To make sure get a consistent time in simulation
155-      ros::Time::waitForValid();
156:      map_resp_.map.info.map_load_time = ros::Time::now();
157-      map_resp_.map.header.frame_id = frame_id_;
158-      map_resp_.map.header.stamp = ros::Time::now();
--
ros/rviz/src/test/map_test.py
17-    grid.header.frame_id = "map"
18-    grid.header.stamp = rospy.Time.now()
19:    grid.info.map_load_time = rospy.Time.now()
20-    grid.info.resolution = 1.0
21-    grid.info.width = 3
</code>```

GridMapRosConverter.cpp deserves credit over the others for for re-using the header stamp and putting in a comment instead of having sequential ros::Time::now() calls for time stamps in the same message which will result in very slightly different timestamps which will confuse the issue of whether there is a meaningful difference between the header stamp and map_load_time.

But still I don't fully understand 'Same as header stamp as we do not load the map', probably looking elsewhere in grid_map_ros would reveal what those authors believed map_load_time to mean.

Body:
I have a lot of ros packages checked out into a big catkin workspace, looks like it is redundant with the header stamp:

$ git grep --no-index -n -C 2 --heading map_load_time
other/jsk_common/jsk_ros_patch/multi_map_server/src/main.cpp
155-      ROS_INFO("Loading map from image \"%s\"", mapfname.c_str());
156-      map_server::loadMapFromFile(&map_resp_,mapfname.c_str(),res,negate,occ_th,free_th, origin);
157:      map_resp_.map.info.map_load_time = ros::Time::now();
158-      map_resp_.map.header.frame_id = frame_id;
159-      map_resp_.map.header.stamp = ros::Time::now();
--
ros/grid_map/grid_map_ros/src/GridMapRosConverter.cpp
275-  occupancyGrid.header.frame_id = gridMap.getFrameId();
276-  occupancyGrid.header.stamp.fromNSec(gridMap.getTimestamp());
277:  occupancyGrid.info.map_load_time = occupancyGrid.header.stamp;  // Same as header stamp as we do not load the map.
278-  occupancyGrid.info.resolution = gridMap.getResolution();
279-  occupancyGrid.info.width = gridMap.getSize()(0);
--
ros/navigation/map_server/src/main.cpp
154-      // To make sure get a consistent time in simulation
155-      ros::Time::waitForValid();
156:      map_resp_.map.info.map_load_time = ros::Time::now();
157-      map_resp_.map.header.frame_id = frame_id_;
158-      map_resp_.map.header.stamp = ros::Time::now();
--
ros/rviz/src/test/map_test.py
17-    grid.header.frame_id = "map"
18-    grid.header.stamp = rospy.Time.now()
19:    grid.info.map_load_time = rospy.Time.now()
20-    grid.info.resolution = 1.0
21-    grid.info.width = 3

GridMapRosConverter.cpp deserves credit over the others for for re-using the header stamp and putting in a comment instead of having sequential ros::Time::now() calls for time stamps in the same message which will result in very slightly different timestamps which will confuse the issue of whether there is a meaningful difference between the header stamp and map_load_time.

But still I don't fully understand 'Same as header stamp as we do not load the map', probably looking elsewhere in grid_map_ros would reveal what those authors believed map_load_time to mean.


问:
How to create a vector of message_filters::Subscriber?
When I try to create a vector of message_filters::Subscriber like so:

```<code>#include <message_filters/subscriber.h>
#include "sensor_msgs/msg/point_cloud2.hpp"

int main(int argc, char * argv[]) {
    std::vector<message_filters::Subscriber<sensor_msgs::msg::PointCloud2>> pc_subscribers_;
    message_filters::Subscriber<sensor_msgs::msg::PointCloud2> sub;
    pc_subscribers_.push_back(sub);
    return 0;
}
</code>```

I get the following error:

```<code>...
/home/sam/work/radar_ws/src/radar_velocity_cpp/src/publisher_radar_ego_comp.cpp:56:31:   required from here
/usr/include/c++/9/ext/new_allocator.h:146:4: error: use of deleted function ‘message_filters::Subscriber<sensor_msgs::msg::PointCloud2_<std::allocator<void> > >::Subscriber(const message_filters::Subscriber<sensor_msgs::msg::PointCloud2_<std::allocator<void> > >&)’
  146 |  { ::new((void *)__p) _Up(std::forward<_Args>(__args)...); }
      |    ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
In file included from /home/sam/work/radar_ws/src/radar_velocity_cpp/src/publisher_radar_ego_comp.cpp:3:
/opt/ros/foxy/include/message_filters/subscriber.h:102:7: note: ‘message_filters::Subscriber<sensor_msgs::msg::PointCloud2_<std::allocator<void> > >::Subscriber(const message_filters::Subscriber<sensor_msgs::msg::PointCloud2_<std::allocator<void> > >&)’ is implicitly deleted because the default definition would be ill-formed:
  102 | class Subscriber : public SubscriberBase, public SimpleFilter<M>
...
</code>```

答:
For the sake of completeness, here are the lines changed based on @Tully's answer</p>
```<code>
#include <message_filters/subscriber.h>
#include "sensor_msgs/msg/point_cloud2.hpp"

int main(int argc, char * argv[]) {
    std::vector<std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::PointCloud2>>> pc_subscribers_;
    pc_subscribers_.push_back(std::make_shared<message_filters::Subscriber<sensor_msgs::msg::PointCloud2>>(this, sensor_topic));
    return 0;
}
</code>```

ContentLicense: CC BY-SA 4.0

Body:
For the sake of completeness, here are the lines changed based on @Tully's answer

#include <message_filters/subscriber.h>
#include "sensor_msgs/msg/point_cloud2.hpp"

int main(int argc, char * argv[]) {
    std::vector<std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::PointCloud2>>> pc_subscribers_;
    pc_subscribers_.push_back(std::make_shared<message_filters::Subscriber<sensor_msgs::msg::PointCloud2>>(this, sensor_topic));
    return 0;
}


问:
Get a rotation to align a vector, n with another vector, a and be able to rotate around a
Let us say I have a rigid body, B. Let us say I have a normal vector n. And let us say I have another direction vector a. I need to rotate my rigid body such that n aligns with a. For the purposes of this question, let us assume that the normal vector n and B share the same frame of reference. I understand this is an under-constrained problem, since the rigid body can be free to rotate around the axis a after being aligned. Let us call this angle of rotation as $\theta$ Is there a way to get this rotation matrix parameterized by $\theta$, such that I can plug in different values of $\theta$ and I can obtain a rigid body pose?

Thanks.

答:
Assuming you are working in 3-dimensions, this is exactly what the cross-product does. To find the vector of rotation that rotates $\mathbf{n} \in \mathbb{R}^{3}$ into $\mathbf{a} \in \mathbb{R}^{3}$, we have:

$$\mathbf{e} = \mathbf{n} \times \mathbf{a} = (||\mathbf{n}||\ ||\mathbf{a}||   \sin(\theta)) \cdot \mathbf{u}$$

where $||\cdot||$ represents the 2-norm, $\theta$ represents the angle of rotation between $\mathbf{a}$ and $\mathbf{n}$, and $\mathbf{u} \in \mathbb{R}^{3}$ represents the axis of rotation. Now, to rotate $\mathbf{n}$ onto $\mathbf{a}$. Via the Rodrigues' rotation formula, we have:

$$\mathbf{n}_{\mathbf{a}} = \cos(\theta) \mathbf{n} + \sin(\theta)(\mathbf{u} \times \mathbf{n}) + (1-\cos(\theta))(\mathbf{u} \cdot \mathbf{n})\mathbf{u}$$

Where $\theta$ and $\mathbf{u}$ are the same values as described originally. We solve for them as:

$$ \mathbf{u} = \frac{\mathbf{e}}{||\mathbf{e}||} \\ \theta = \arccos(\frac{\mathbf{n} \cdot \mathbf{a}}{||\mathbf{n}|| \ ||\mathbf{a}||})$$

It should be noted that we use $\arccos$ instead of $\arcsin$ as this will return an angle from $0$ to $\pi$, which is a better representation of the angle between two vectors. You might notice that $\mathbf{e} = \mathbf{0}$ when $\theta = \pi$. This is the case as the cross product is undetermined in these configurations (we have an infinite number of vectors that we could rotate about to achieve our goal). As you mentioned, this is one of the issues with the problem you have posed, but luckily it only occurs in one specific case and hard-coded solutions can be made to handle this fringe case.

Now, to obtain a rotation matrix that will perform the same rotation for the rest of the vectors associated with body B, we have:

$$ \mathbf{R} = \exp(\mathbf{K}\theta) = \mathbf{I} + \sin(\theta)\mathbf{K} + (1-\cos(\theta))\mathbf{K}^{2} $$

where $\mathbf{K} = [\mathbf{u}]_{\times}$ is the cross-product matrix:

$$ \mathbf{K} \triangleq \left[ \begin{array}{ccc} 0 & -u_{3} & u_{2} \\ u_{3} & 0 & -u_{1} \\ -u_{2} & u_{1} & 0  \end{array} \right]$$

Again, you will need to handle the case where $\mathbf{n}$ and $\mathbf{a}$ are parallel and opposite direction as an independent case, solving for $\mathbf{u}$ and $\theta$ still. Once this is handled, this method should work generally for any $\mathbf{n}$ and $\mathbf{a}$.


问:
How to: Attach wheel encoder to motor?
I have a couple of these DC motors

http://www.pololu.com/product/2202

Which have an extended motor shaft that sticks out the back and is 1mm diameter.

I'm having trouble trying to think of the best way to attach an encoder disk to this shaft.

I thought of getting a custom wheel 3d printed and make the opening 0.9mm so it will be a tight fit. But I don't know if is just to small?

I also though of taking the encoder disks from a PC mouse and drilling a 1mm / 0.9mm but its the same problem but with the added difficultly of trying to drill a small hole on a small thing.

So I wondered if anyone knows a better way, or of a made disk to attach. As I just can't find anything for a 1mm shaft

答:
Place a disk on the large shaft.

Depends how 'professional' you want to be.

A 1mm hole is not unreasonably hard to drill using a small hobby drill press, and you can do it with many battery drills with due care - the chuck minimum drill size is the main limitation. Down to about 0.8mm is easily enough achievable by mere mortals.

Substitute imperial units below if the mood seizes you.

(2a) You can push the 1mm shaft into material that it will penetrate and glue it in place with either epoxy or a super super-glue. Go from there.

(2b)  If you get a piece of say 4mm dia brass rod you can  -

File one end flat - repeat until good enough.

Mark centre - many guides how to do this on net.

Centre punch centre

Drill 1mm hole in end. Drill in horizontal clamp may help. Or not.
Drill and tap to 2mm or 2.5mm a hole at right angles to long axis.

Add grub screw or screw - 2 or 2.5mm to suit.

You now have a shaft extension and half a coupler. Use other end as seems good.

(3) Mark a dark line on shaft. Use optical reflective detector to produce tach signal.

OwnerDisplayName: binarysmacker

ContentLicense: CC BY-SA 3.0

Title: How to: Attach wheel encoder to motor?

OwnerDisplayName: Russell McMahon


问:
Object tracking using PTZ camera with PID Controller
I'm trying hard to implement an object-tracking algorithm on a PTZ camera. For this, I have chosen to use a PID controller for error correction.

In my case, the set point value is the center of the image. I will get the error term from an image processing algorithm. Using the error term, I have to center the object in the image plane by moving the camera's azimuth and elevation.

When I tried to track a static object, the PID controller worked fine resulting in a null steady-state error.

Here below are the points I'm unable to understand:

How to choose kP, kI, kD values during tuning.

Whether the below code snippet is suitable for my application.
```<code>    currTime = time. Time()
    deltaTime = currTime - prevTime
    # delta error
    deltaError = error - prevError
    # proportional term
    cP = error
    # integral term
    cI += error * deltaTime
    # derivative term and prevent divide by zero
    cD = (deltaError / deltaTime) if deltaTime > 0 else 0
    # save previous time and error for the next update
    prevTime = currTime
    prevError = error

    opt = sum([
        self.kP * self.cP,
        self.kI * self.cI,
        self.kD * self.cD])
</code>```

Should I wait some time after calculating the PID output? Or, can I update the PID immediately?

Should I control the speed of the PTZ camera?

@Ugo Pattacini, can you please help with this?

答:
This is a very related resource: Algorithm for centering an object that is changing speed.

The PID controller is a viable choice. The two channels (tilt and pan) are independent and so are the two PID controllers you need to implement, which will each act on one single channel.

When I tried to track a static object, the PID controller worked fine resulting in a null steady-state error.

This is somehow expected as a moving object will add its own unknown dynamics to the game, making the controller's life harder.

How to choose kP, kI, kD values during tuning.

The key here is to use a PI controller. As suggested in the related resource, start off with tuning a simple P controller, in order to get closer to centering the target in the image plane. You will certainly end up with a residual error that, in turn, you can compensate for by slowly increasing the integral gain I.

The P has to make most of the job. A simple P is not enough though. Imagine a situation where the object moves at a constant velocity and you've already got it in the center of the image. The error is 0 and so is the controller's effort $K_p \cdot 0 = 0$; hence, you don't move the camera. This tells us that the integral is strictly required since it can deliver nonnull effort for null input error.

The derivative part D serves as a prediction that would help us catch up with where the object will most likely be in the next few controller's time instants. Thereby, the derivative contribution can reduce overshoots and make the controller more reactive. However, predictions work in practice only for objects that move, well, predictably; otherwise, derivative can harm the system performance. For this reason, try your best without D, tuning up a PI.

Whether the below code snippet is suitable for my application.

The snippet looks overall good, although it implements a quite simplistic PID. The only point I'd like to stress is that you ought to stick to a fixed controller's sample time $T_s$. Don't try to adjust for the elapsed time with deltaTime. The latter variable needs to be a constant deltaTime = Ts. To this end, you have to call the snippet with the said periodicity by means of an interrupt service routine or by relying on OS-specific threads, for example.

Should I wait some time after calculating the PID output? Or, can I update the PID immediately?

The PID output needs to be applied to the system with the periodicity of $T_s$. As discussed right above, you should resort to interrupts or threads, which will guarantee some sort of robust timing. All modern languages offer threading through their libraries.

$T_s$ is usually in the range of milliseconds, but really depends on the system at hand.

Should I control the speed of the PTZ camera?

Nope. You close the loop around the pixels error and this is sufficient.
Anyway, the output of the PID is typically a voltage to be applied to an electrical motor. In this context, setting the voltage amounts to controlling the velocity of the motor shafts (up to a certain extent), and thus of the camera's channels.

More generally, there are configurations where nested control loops are employed. See for example Inner current controller benefits.

A final note on the Z part of the PTZ camera. You may need to have different sets of PID gains for different zoom configurations.


问:
How to import 3D models in Webots?
I have 3D models with UV mapping and textures in Blender and I would like to import them in Webots. Can you please recommend the best way to do this?</p>

答:
First you should export your Blender model to either obj(.obj) or Collada(.dae) format.

In Webots, you can then use the CadShape node. It is really simple and straightforward to use: just put the path to your .dae/.obj file in its url field. For more information see the documentation here.

Your model should now be visible in Webots with its textures.

If you want more control over the different parts of the model, you can import each different mesh in a Mesh node and then define a specific appearance for each mesh.

Title: How to import 3D models in Webots?


问:
Read device value from another robot controller in Webots
I'm using Webots R2022b and I would like to have a supervisor controller that applies an algorithm on the camera data recorded by the Mavic2Pro drone on an another robot.
But, when calling the getDevice function in the supervisor controller, the device is not found.

Is there any way to get the Mavic2Pro camera device from the supervisor controller?

Disclaimer: I am a Webots developer working at Cyberbotics.
This is a question we get very often and I hope the answer can help other users.

答:
In Webots, there is only one controller program per Robot node. The controller program can access only the devices (sensors and actuators) listed in its children field. Therefore it is not possible for a robot controller to access the device of another robot. So, if you would like that a supervisor robot controller processes the images taken by the Mavic2Pro robot, you should get the images in the Mavic2Pro controller and send them to the supervisor robot controller using for example an Emitter/Receiver system.

Alternatively, you may want to change the default content of the cameraSlot of the Mavic2Pro proto and insert a supervisor Robot node that would have its own controller and contain a Camera node in its children list:

Mavic2Pro {
  controller "mavic_controller"
  cameraSlot Robot {
    supervisor TRUE
    controller "supervisor_controller"
    children Camera {
      width 400
      height 240
    }
  }
}
```

In that case however, the mavic_controller won't be able to access the camera device as it will be the exclusive property of the supervisor Robot.

Body:
In Webots, there is only one controller program per Robot node. The controller program can access only the devices (sensors and actuators) listed in its children field. Therefore it is not possible for a robot controller to access the device of another robot. So, if you would like that a supervisor robot controller processes the images taken by the Mavic2Pro robot, you should get the images in the Mavic2Pro controller and send them to the supervisor robot controller using for example an Emitter/Receiver system.

Alternatively, you may want to change the default content of the cameraSlot of the Mavic2Pro proto and insert a supervisor Robot node that would have its own controller and contain a Camera node in its children list:

Mavic2Pro {
  controller "mavic_controller"
  cameraSlot Robot {
    supervisor TRUE
    controller "supervisor_controller"
    children Camera {
      width 400
      height 240
    }
  }
}

In that case however, the mavic_controller won't be able to access the camera device as it will be the exclusive property of the supervisor Robot.


问:
Convert Ros2 Foxy Message to Ros2 Humble
I currently have 2 projects running, 1 is using Ros2 Foxy and the other is Ros2 Humble, it seems like Ros2 Humble isnt’ entirely compatible with reading old message Format, e.g: I cant read message of type visualization_msgs:MarkerArray , is this a known issue or does any one knows any workaround? I'm thinking of maybe creating a node that subscribe to the required topic in Foxy and maybe convert it into a format that Humble can read?

Any help is much appreciated.

答: The only way I can surmise there being any issues between ROS2 versions is if the message definition itself changes. If there is truly a ROS2 version issue, create a simple message publisher in one version via CLI with ros2 topic pub and then ros2 topic echo the message via the other ROS2 version. If that pub/sub operation works, then your error is not the message itself. The information you gave in the question is not enough to answer further.


问:
Estimating angular velocity from position
If you have two position estimates $P_{t_1}$ and $P_{t_2}$ and times $t_1$ and $t_2$ how do you extract the average angular velocity from them? The linear velocity is a simple difference but how do you compute angular velocity?

答:
You don't

In order to have an angular velocity, you have to be able to show that there is an arc in the trajectory. Representing arcs requires 3 points on the trajectory, and you only have 2. You need 1 more point or one more piece of information to get the angular velocity.


问:
Lookup would require extrapolation into the future on the same machine
I get this error when trying to view my RPLidar A2 scan in rviz2. It’s on a differential drive robot controlled using arduino using Articulated Robotics’ code that I quickly ported to Humble, alongside the controller manager code. I am running everything on a laptop, no separate machines, on ROS2 humble.
It’s connected to the base_link via the URDF but I tried a static transform publisher but the same thing occurred.

It shows perfectly fine when viewing the base_link frame, but with Odom the issue occurs.

Oddly enough when I use a YDLIDAR G4, it works intermittently but it loses transform often. When I use the rplidar it doesn’t work at all. This is my first time using ros2 so I am confused

```<code>[ERROR] [1668990540.121377402] [rviz2]: Lookup would require extrapolation into the future. Requested time 1668990540.179102 but the latest data is at time 1668990540.113331, when looking up transform from frame [laser_frame] to frame [odom]
</code>```

答: You need to synchronize the clocks on the host machines using tools like tzdata (for Linux hosts). This is not so much a code problem as it is a network/host problem. Make sure all devices are connected to the same wireless network and synchronize clocks.


问:
Spdlog linker error with ROS2 packages
Good afternoon!

We are a company that has a pretty big robotic framework and we are currently working in a transition into ROS2. We are first creating the structure that all of our packages will follow. Until now, these were separate modules in the same CMake project. Now, as we are starting to test the transition we encountered an issue.

Explain the problem

As we are moving all of our framework, we have our own logger that is being used in our development. Transitioning into the ROS2 logger in all of our modules is not feasible right now. Both loggers use Spdlog under the hood (ROS2 logger and our logger). The conversion of this module is correct and everything works great downstream. However when we try to create a package that has a dependency in ROS (for the creation of a ROS node) we encounter some linker errors.

This error appears only when linking executables that depend on ROS2. If we compile executables that exclusively depend on our modules (No ROS2 dependencies) then everything works correctly

Our Spdlog installation is built from source with version v1.9.2, while I believe ROS uses the binary installation of libspdlog-dev which has version v1.5.0. I've tried to upgrade and downgrade our version with no effect and also simply installing the ROS version to no effect.

All of these linker errors refer to the fmt library which Spdlog depends on.

Linker Error

This is the linker error in question. It's repeated in the terminal quite a few times. I've changed the names of the home paths and files names but the rest is as shown.

```<code>/path/ros2_galactic/ros2-linux/include/spdlog/fmt/bundled/format.h:1051: undefined reference to `fmt::v7::detail::assert_fail(char const*, int, char const*)'
/usr/bin/ld: CMakeFiles/ProjectSample.dir/samples/ProjectSample.cpp.o: in function `spdlog::details::elapsed_formatter<spdlog::details::null_scoped_padder, std::chrono::duration<long, std::ratio<1l, 1000000l> > >::format(spdlog::details::log_msg const&, tm const&, fmt::v7::basic_memory_buffer<char, 250ul, std::allocator<char> >&)':
/usr/include/x86_64-linux-gnu/bits/string_fortified.h:34: undefined reference to `fmt::v7::detail::basic_data<void>::digits'
/usr/bin/ld: CMakeFiles/ProjectSample.dir/samples/ProjectSample.cpp.o: in function `std::make_unsigned<long>::type fmt::v7::detail::to_unsigned<long>(long)':
/path/ros2_galactic/ros2-linux/include/spdlog/fmt/bundled/core.h:326: undefined reference to `fmt::v7::detail::assert_fail(char const*, int, char const*)'
/usr/bin/ld: CMakeFiles/ProjectSample.dir/samples/ProjectSample.cpp.o: in function `fmt::v7::detail::count_digits(unsigned long)':
/path/ros2_galactic/ros2-linux/include/spdlog/fmt/bundled/format.h:924: undefined reference to `fmt::v7::detail::basic_data<void>::zero_or_powers_of_10_64_new'
/usr/bin/ld: CMakeFiles/ProjectSample.dir/samples/ProjectSample.cpp.o: in function `spdlog::details::elapsed_formatter<spdlog::details::null_scoped_padder, std::chrono::duration<long, std::ratio<1l, 1l> > >::format(spdlog::details::log_msg const&, tm const&, fmt::v7::basic_memory_buffer<char, 250ul, std::allocator<char> >&)':
/usr/include/x86_64-linux-gnu/bits/string_fortified.h:34: undefined reference to `fmt::v7::detail::basic_data<void>::digits'
/usr/bin/ld: CMakeFiles/ProjectSample.dir/samples/ProjectSample.cpp.o: in function `fmt::v7::detail::count_digits(unsigned long)':
/path/ros2_galactic/ros2-linux/include/spdlog/fmt/bundled/format.h:924: undefined reference to `fmt::v7::detail::basic_data<void>::zero_or_powers_of_10_64_new'
/usr/bin/ld: CMakeFiles/ProjectSample.dir/samples/ProjectSample.cpp.o: in function `fmt::v7::detail::format_decimal_result<char*> fmt::v7::detail::format_decimal<char, unsigned long>(char*, unsigned long, int)':
</code>```

It seems to come from "/path/ros2_galactic/ros2-linux/include/spdlog/fmt/bundled/", thus we guessed the problem comes from the Spdlog installed from ROS. I was wondering if someone has faced this issue and/or maybe knows why it's caused or has any idea on how to solve it.

Tried solutions

As I've said, we tried installing different versions of spdlog and removing ours just to have the ROS2 one but the error persists.

We've also tried making changes to the CMake but the linkage seems correct and follows the instructions of spdlog. I would prefer not to show the CMake files unless needed as they are sensitive and can't be shared as they are.

EDIT

Hello, to try to understand a bit better my structure I've added the basic CMake structure of the packages that I'm doing

```<code>cmake_minimum_required(VERSION 3.11...3.24)

project(
    moduleName
    VERSION 1.0.0
    DESCRIPTION "Add a short description of your module"
    LANGUAGES CXX)

set(CMAKE_CXX_STANDARD 17)
set_property(GLOBAL PROPERTY USE_FOLDERS ON)

# Declare options
option(BUILD_TESTS "Build the Tests of the module" ON)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
    set(COMPILER_FLAGS -Wall -Wextra -Wpedantic -Wshadow -pthread)
endif()

if (NOT CMAKE_BUILD_TYPE)
    set(CMAKE_BUILD_TYPE "Debug")
endif()

include(GNUInstallDirs)
include(${CMAKE_CURRENT_SOURCE_DIR}/cmakeutilities/CMakeFunctions.cmake)

if(<span class="math-container">${CMAKE_BUILD_TYPE} STREQUAL "Debug")
add_compile_options($</span>{COMPILER_FLAGS} -O -g)
else()
    add_compile_options(${COMPILER_FLAGS} -O2)
endif()

set(EXPORT_NAME "${PROJECT_NAME}_export")

find_package(ament_cmake REQUIRED)

################
# Dependencies #
################

find_package(dependency1 REQUIRED)
find_package(dependency2 REQUIRED)
find_package(dependency3 REQUIRED)

set(MODULE_DEPENDENCIES
    dependency1
    dependency2
    dependency3
)

#################
# Build library #
#################

add_library(<span class="math-container">${PROJECT_NAME} SHARED
./src/ModuleSource.cpp)
target_include_directories($</span>{PROJECT_NAME}
    PUBLIC
        <span class="math-container">$<BUILD_INTERFACE:$</span>{CMAKE_CURRENT_SOURCE_DIR}/include>
        <span class="math-container">$<BUILD_INTERFACE:$</span>{CMAKE_BINARY_DIR}>
        <span class="math-container">$<INSTALL_INTERFACE:$</span>{CMAKE_INSTALL_INCLUDEDIR}>
    PRIVATE
        ${CMAKE_CURRENT_LIST_DIR}
)

###################
# Install Library #
###################

# Linking
ament_target_dependencies(<span class="math-container">${PROJECT_NAME} $</span>{MODULE_DEPENDENCIES})

set_target_properties(<span class="math-container">${PROJECT_NAME}
PROPERTIES VERSION $</span>{PROJECT_VERSION} SOVERSION ${PROJECT_VERSION_MAJOR}
)

# Install Library Target
install(
    TARGETS <span class="math-container">${PROJECT_NAME}
EXPORT $</span>{EXPORT_NAME}
    LIBRARY DESTINATION <span class="math-container">${CMAKE_INSTALL_LIBDIR}
ARCHIVE DESTINATION $</span>{CMAKE_INSTALL_LIBDIR}
    RUNTIME DESTINATION <span class="math-container">${CMAKE_INSTALL_BINDIR}
INCLUDES DESTINATION $</span>{CMAKE_INSTALL_INCLUDEDIR}
)

# Install Headers
install(
    DIRECTORY <span class="math-container">${PROJECT_SOURCE_DIR}/include/
DESTINATION $</span>{CMAKE_INSTALL_INCLUDEDIR}
)

###############
# Executables #
###############

add_executable(ModuleSample samples/ModuleSample.cpp)
target_link_libraries(ModuleSample ${PROJECT_NAME})

install(
    TARGETS ModuleSample
    DESTINATION <span class="math-container">${CMAKE_INSTALL_LIBDIR}/$</span>{PROJECT_NAME}
)

##############
# Test Suite #
##############

if((BUILD_TESTING OR BUILD_TESTS) AND LIBRARY_HAS_TESTS)
    find_package(ament_lint_auto REQUIRED)
    find_package(ament_cmake_gmock REQUIRED)
    #ament_lint_auto_find_test_dependencies()

    ament_add_gmock(ut-<span class="math-container">${PROJECT_NAME}
    ./tests/ModuleTests.cpp)
target_link_libraries(ut-$</span>{PROJECT_NAME} ${PROJECT_NAME})
endif()

##################
# Export Targets #
##################

ament_export_dependencies(<span class="math-container">${MODULE_DEPENDENCIES})
ament_export_targets($</span>{EXPORT_NAME} HAS_LIBRARY_TARGET)
ament_export_include_directories(include)

ament_package()

</code>```

Edit 2

Hello! The full structure would be something like the following. I have two packages. The first one is my own custom logger and the CMake structure is as follows:

```<code>cmake_minimum_required(VERSION 3.11...3.24)

project(
    customLogger
    VERSION 1.0.0
    DESCRIPTION "Add a short description of your module"
    LANGUAGES CXX)

set(CMAKE_CXX_STANDARD 17)
set_property(GLOBAL PROPERTY USE_FOLDERS ON)

# Declare options
option(BUILD_TESTS "Build the Tests of the module" ON)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
    set(COMPILER_FLAGS -Wall -Wextra -Wpedantic -Wshadow -pthread)
endif()

if (NOT CMAKE_BUILD_TYPE)
    set(CMAKE_BUILD_TYPE "Debug")
endif()

include(GNUInstallDirs)
include(${CMAKE_CURRENT_SOURCE_DIR}/cmakeutilities/CMakeFunctions.cmake)

if(<span class="math-container">${CMAKE_BUILD_TYPE} STREQUAL "Debug")
add_compile_options($</span>{COMPILER_FLAGS} -O -g)
else()
    add_compile_options(${COMPILER_FLAGS} -O2)
endif()

set(EXPORT_NAME "${PROJECT_NAME}_export")

find_package(ament_cmake REQUIRED)

################
# Dependencies #
################

find_package(spdlog REQUIRED)

#################
# Build library #
#################

add_library(<span class="math-container">${PROJECT_NAME} SHARED
./src/ModuleSource.cpp)
target_include_directories($</span>{PROJECT_NAME}
    PUBLIC
        <span class="math-container">$<BUILD_INTERFACE:$</span>{CMAKE_CURRENT_SOURCE_DIR}/include>
        <span class="math-container">$<BUILD_INTERFACE:$</span>{CMAKE_BINARY_DIR}>
        <span class="math-container">$<INSTALL_INTERFACE:$</span>{CMAKE_INSTALL_INCLUDEDIR}>
    PRIVATE
        ${CMAKE_CURRENT_LIST_DIR}
)

###################
# Install Library #
###################

# Linking
ament_target_dependencies(${PROJECT_NAME} spdlog)

set_target_properties(<span class="math-container">${PROJECT_NAME}
PROPERTIES VERSION $</span>{PROJECT_VERSION} SOVERSION ${PROJECT_VERSION_MAJOR}
)

# Install Library Target
install(
    TARGETS <span class="math-container">${PROJECT_NAME}
EXPORT $</span>{EXPORT_NAME}
    LIBRARY DESTINATION <span class="math-container">${CMAKE_INSTALL_LIBDIR}
ARCHIVE DESTINATION $</span>{CMAKE_INSTALL_LIBDIR}
    RUNTIME DESTINATION <span class="math-container">${CMAKE_INSTALL_BINDIR}
INCLUDES DESTINATION $</span>{CMAKE_INSTALL_INCLUDEDIR}
)

# Install Headers
install(
    DIRECTORY <span class="math-container">${PROJECT_SOURCE_DIR}/include/
DESTINATION $</span>{CMAKE_INSTALL_INCLUDEDIR}
)

###############
# Executables #
###############

add_executable(ModuleSample samples/ModuleSample.cpp)
target_link_libraries(ModuleSample ${PROJECT_NAME})

install(
    TARGETS ModuleSample
    DESTINATION <span class="math-container">${CMAKE_INSTALL_LIBDIR}/$</span>{PROJECT_NAME}
)

##############
# Test Suite #
##############

if((BUILD_TESTING OR BUILD_TESTS) AND LIBRARY_HAS_TESTS)
    find_package(ament_lint_auto REQUIRED)
    find_package(ament_cmake_gmock REQUIRED)
    #ament_lint_auto_find_test_dependencies()

    ament_add_gmock(ut-<span class="math-container">${PROJECT_NAME}
    ./tests/ModuleTests.cpp)
target_link_libraries(ut-$</span>{PROJECT_NAME} ${PROJECT_NAME})
endif()

##################
# Export Targets #
##################

ament_export_dependencies(<span class="math-container">${MODULE_DEPENDENCIES})
ament_export_targets($</span>{EXPORT_NAME} HAS_LIBRARY_TARGET)
ament_export_include_directories(include)

ament_package()

</code>```

The CMake from the second package is identical to the first one except that it depends on the custom logger:

```<code>find_package(customLogger REQUIRE)

ament_target_dependencies(${PROJECT_NAME} customLogger)
</code>```

If, instead of:

```<code>ament_target_dependencies(${PROJECT_NAME} spdlog)
</code>```

We write:

```<code>target_link_libraries(${PROJECT_NAME} spdlog::spdlog)
</code>```

Then we get a different linker error:

```<code>/ros2_galactic/ros2-linux/include/spdlog/logger.h:168: undefined reference to `spdlog::details::log_msg::log_msg(spdlog::source_loc, fmt::v7::basic_string_view<char>, spdlog::level::level_enum, fmt::v7::basic_string_view<char>)'
/usr/bin/ld: /path/ros2_galactic/ros2-linux/include/spdlog/logger.h:168: undefined reference to `spdlog::details::log_msg::log_msg(spdlog::source_loc, fmt::v7::basic_string_view<char>, spdlog::level::level_enum, fmt::v7::basic_string_view<char>)'
/usr/bin/ld: /path/ros2_galactic/ros2-linux/include/spdlog/logger.h:168: undefined reference to `spdlog::details::log_msg::log_msg(spdlog::source_loc, fmt::v7::basic_string_view<char>, spdlog::level::level_enum, fmt::v7::basic_string_view<char>)'
</code>```

Still, it seems to be finding the spdlog installation of ROS2 instead of the one we use.

Changing the find package into:

```<code>find_package(spdlog 1.9.2 REQUIRED)
</code>```

makes it so that it finally finds our installation instead of the ROS2 one but downstream it does not seem to find it anymore and it keeps throwing the same linker errors.

答:
From the answer above from Chuck I see you got at least the basic capabilities working. However I'd like to suggest that you may want to consider an alternative approach. Relying on two different versions of a binary library in potentially the same compiled unit is likely to be a fragile balance and take a lot of work to maintain. I suggest considering an alternative approach.

A much more robust mechanism for doing this is to setup your environment with the alternative version and compile everything against that version. This will avoid any potential conflict and very hard to debug ABI mismatches down the line. You're somewhat getting lucky that you're getting linker errors at the moment. Even more insidious would be ABI incompatibilities which would manifest themselves as crashes which the debugger might not even be able to backtrace correctly if you force different versions of the same library into compile units.

Title: Spdlog linker error with ROS2 packages


答:
It's not that the transform doesn't exist. It's that the node doesn't know about the transform yet when you query for the data.

With that small of a delta the usual problem is that the data gets there faster than the transform information can be communicated. You need to hold the data for ~ 0.01 seconds until the transforms are available.

The canonical way to do this is to subscribe to the topic via a tf2_ros::MessageFilter which will subscribe to your incoming topic and give you the callback when the data has arrived. There's a tutorial on the topic here

One reason you may see a slight difference between the two lidars could be that one may be providing the additional time resolution of interbeam distances in which case you'll want to use the tolerance parameter to make sure to cover that too.

Title: Lookup would require extrapolation into the future on the same machine

Body:
I get this error when trying to view my RPLidar A2 scan in rviz2. It’s on a differential drive robot controlled using arduino using Articulated Robotics’ code that I quickly ported to Humble, alongside the controller manager code. I am running everything on a laptop, no separate machines, on ROS2 humble.
It’s connected to the base_link via the URDF but I tried a static transform publisher but the same thing occurred.

It shows perfectly fine when viewing the base_link frame, but with Odom the issue occurs.

Oddly enough when I use a YDLIDAR G4, it works intermittently but it loses transform often. When I use the rplidar it doesn’t work at all. This is my first time using ros2 so I am confused

```<code>[ERROR] [1668990540.121377402] [rviz2]: Lookup would require extrapolation into the future. Requested time 1668990540.179102 but the latest data is at time 1668990540.113331, when looking up transform from frame [laser_frame] to frame [odom]
</code>```


问:
Why is Webots running so slow on my computer?
The provided sample simulations are running very slowly in Webots. How can I improve this?

I am using Webots R2022b on Ubuntu 22.04.

答:
Did you check if your graphics driver are installed correctly and up-to-date?

You can find some instructions to verify the graphics driver installation for Webots here.
Note that it is recommended to run Webots on computers equipped with recent NVIDIA or AMD graphics cards.

You might also try other tweaks to speed up your machine:

on laptop computer you should check the power options and make sure to use the high performance power plan

disable the Ubuntu desktop animations (from the "Accessibility" section in the Settings app)  that could use ample system resources, especially on older computers, and cause a huge performance drop of the rendering system.settings


问:
Is it possible to immobilize one part of a robot?
I need to fix one part of my robot to the static environment. Can this be done in Webots?
I'm using R2022b.</p>

答:
Yes it is possible, you have two solutions:

If you want to modify your robot such that one part is always pinned to the static environment, you could remove the Physics node from the top Robotnode.

Then you can add Physics nodes only in the parts that you want.

This is what is done in this example with the staticBase option: the base is static but the robotic arm have physics.

If you do not want to modify your robot or want to only immobilize your robot temporary, you can do it with a physics plugin. You can add a physics plugin with the File / New / New Physics Plugin... menu item. In the plugin code, you must simply add an ODE fixed joint between the dBodyID of the robot part and the static environment. This can be implemented like this:

```<code>#include <ode/ode.h>
#include <plugins/physics.h>

void webots_physics_init() {

 // get body of the robot part
 dBodyID body = dWebotsGetBodyFromDEF("MY_ROBOT_PART");

 // get the matching world
 dWorldID world = dBodyGetWorld(body);

 // the joint group ID is 0 to allocate the joint normally
 dJointID joint = dJointCreateFixed(world, 0);

 // attach robot part to the static environment: 0
 dJointAttach(joint, body, 0);

 // fix now: remember current position and rotation
 dJointSetFixed(joint);
}

void webots_physics_step() {
 // nothing to do
}

void webots_physics_cleanup() {
 // nothing to do
}
</code>```

You will find the description of Webots physics plugin API here. You will find the description about the ODE functions on this page.

Body:
Yes it is possible, you have two solutions:

If you want to modify your robot such that one part is always pinned to the static environment, you could remove the Physics node from the top Robotnode.

Then you can add Physics nodes only in the parts that you want.

This is what is done in this example with the staticBase option: the base is static but the robotic arm have physics.

If you do not want to modify your robot or want to only immobilize your robot temporary, you can do it with a physics plugin. You can add a physics plugin with the File / New / New Physics Plugin... menu item. In the plugin code, you must simply add an ODE fixed joint between the dBodyID of the robot part and the static environment. This can be implemented like this:

#include <ode/ode.h>
#include <plugins/physics.h>

void webots_physics_init() {

 // get body of the robot part
 dBodyID body = dWebotsGetBodyFromDEF("MY_ROBOT_PART");

 // get the matching world
 dWorldID world = dBodyGetWorld(body);

 // the joint group ID is 0 to allocate the joint normally
 dJointID joint = dJointCreateFixed(world, 0);

 // attach robot part to the static environment: 0
 dJointAttach(joint, body, 0);

 // fix now: remember current position and rotation
 dJointSetFixed(joint);
}

void webots_physics_step() {
 // nothing to do
}

void webots_physics_cleanup() {
 // nothing to do
}

You will find the description of Webots physics plugin API here. You will find the description about the ODE functions on this page.


问:
Ros2 and communication - How do you post a Ros2 message in a text box in Unity?
I am developing a project with ros_msft_mrtk and would like to send simple messages via Ros2 to Unity or then to the Hololens2. Unfortunately, I don't understand Ros2's communication. I use the SubscriptionExample, added it to a GameObject and now want to display sented messages in a text field in Unity. That was my attempt:

I get the error: Cannot implicitly convert type 'std_msgs.msg.String' to 'string'. How can I store a received std_msgs.msg.String in a string to output it in Unity and later on the HoloLens?

答:
From the tutorial that you linked. The string value that you want is in the data field of the message msg.Data not the message named std_msgs/String

You can see the message definition here.


问:
How to maximize top speed potential for a given motor?
I have an engineering background however I am new to motors. I am looking to understand what the options exist for maximizing the top speed a motor can produce (say for an RC vehicle) without simply using the "brute force" option of "buy a bigger, heavier, more expensive motor with a higher number of maximum RPMs". Because after a point there is a step function in terms of size and cost that make selection impractical depending on application.

Seems like there is plenty of motorization in the world, so this has to be a solved problem--I just need some pointers to pursue learning more. Specifically: what options exist to maximize motor output for speed?  What are the advantages and disadvantages of each?

All useful help appreciated--thank you!! Happy to clarify if anything is not clear.

答:
Through my own research since originally posting I have learned what seems to be a rather practical answer.  While one could provide some sort of a mechanical advantage in conjunction with a motor, the reality is that it would be less efficient than simply using a larger motor.

The tradeoff is essentially that losses of a larger motor are merely electrical in nature, whereas the losses of nearly any conceivable mechanical advantage (for a specific example, let's suppose a step-up gearbox) will likely be in the form of heat loss.  Another potential "loss" could be in quality or reliability issues due to introducing more moving parts into the system.  So at the end of the day, especially from a systems-level perspective, trading up in motor size, construction, or performance will almost always provide the better option.  I say "almost always", because there very well could be an overly specific application out there where the only option is to pursue a mechanical advantage (such as if you had no control over choice of motor, or other odd physical space constraints).

In conclusion, the old adage still applies: sometimes all you need is simply a larger "hammer".


问:
How to play ROS bag file frame by frame
```<code>rosbag play bagfile.bag
</code>```

ROS's bag player provides a nice tool to replay the data but it's kind of difficult for debugging as we can't control frame-by-frame play. I know it's possible to control replay speed by the -r option but is it possible to play it frame-by-frame? e.g in rqt_bag we can move frame by frame to see the value (although it can't publish the single frame).

How do you debug your program with ros bag file?

答:
You can start your bag in a paused state using

rosbag play bagfile.bag --pause
```

And then you can step through your bag by pressing the s key.
I believe each step is a message from the bag file.

Title: How to play ROS bag file frame by frame

Body:
You can start your bag in a paused state using

rosbag play bagfile.bag --pause

And then you can step through your bag by pressing the s key.
I believe each step is a message from the bag file.


答:
Clarifying the question

The question needs more clarity. For example, you need to explain the positions are of what, with respect to what, resolve in which frame.

Let's assume the positions $P_{t_{1}}$ and $P_{t_{2}}$ are positions of some arbitrary point $b$ on the robot/body, with respect to an arbitrary point $a$ in the world (e.g., origin of the map). Furthermore, let's assume the positions are resolved in the world frame $\mathcal{F}_{w}$.
The position of the robot/body $b$ with respect to the point $a$ at time $t$, resolved in the world frame $\mathcal{F}_{w}$ can then be described as $\mathbf{r}^{b_{t_1}a}_{w}$ and $\mathbf{r}^{b_{t_2}a}_{w}$.

Angular velocity from a motion model

The angular velocity describes the rate of change of frames, and not of points.
Thus, asking for the angular velocity from two points is an ill-posed question.
Perhaps what you meant to be asking is the angular velocity of a rigid body, which has a reference frame, given the rigid body's position.

To compute the angular velocity of a rigid body given it's position requires using its motion model.
The motion model is usually in the form
$$
\begin{align}
\mathbf{x}_{k} &= \mathbf{f}(\mathbf{x}_{k-1}, \mathbf{u}_{k-1}),
\end{align}
$$
where $\mathbf{x}_{k}$ is the state and $\mathbf{u}_{k}$ is the control input  at time $t_{k}$, which may include the angular velocity.
Thus, computing the angular velocity would require computing $\mathbf{u}_{k-1}$ from $\mathbf{x}_{k-1}$ and $\mathbf{x}_{k}$.
If $\mathbf{f}$ is invertible, then the control-input is computed using
$$
\mathbf{u}_{k-1}
= \mathbf{f}^{-1}(\mathbf{x}_{k}, \mathbf{k-1}).
$$

Let's consider a 2D rigid body example, where the state is the robot's displacements $\mathbf{r}^{b_{k}a}_{w}$ and its heading $\theta_{k}$ at time $t_{k}$.
This state can be "nicely" described using the special Euclidean group $SE(2)$ (check Sola - A Micro Lie Theory for more info on Lie groups for robotics):
$$
\begin{align}
\mathbf{T} &=
\begin{bmatrix}
\cos(\theta) & -\sin(\theta) & x \\
\sin(\theta) &  \cos(\theta) & y \\
0 & 0 & 1
\end{bmatrix}
=
\begin{bmatrix}
\mathbf{C}(\theta) & \mathbf{r}^{ba}_{w} \\
\mathbf{0} & 1
\end{bmatrix}
\in SE(2).
\end{align}
$$
Let the control inputs be the linear velocity of the robot resolved in the body frame $\mathbf{v}^{b_{k}a}_{b_{k}}$ and the angular velocity $\omega^{b_{k}w}$.
The motion model is then
$$
\mathbf{T}_{k}
=
\mathbf{T}_{k-1}
\begin{bmatrix}
\mathbf{C}(\Delta t_{k-1} \omega^{b_{k-1}w}) & \Delta t_{k-1} \mathbf{v}^{b_{k-1}a}_{b_{k-1}} \\
\mathbf{0} & 1
\end{bmatrix},
$$
where $\Delta t_{k-1} = t_{k} - t_{k-1}$ is the sampling period and $\mathbf{C}(\cdot)\in SO(2)$ is the rotation matrix constructed from $\Delta t \omega^{b_{k-1}w}$.

Going back to the original question, if you have the full states at times $t_{k}$ and $t_{k-1}$, which include the displacements and headings, then the angular velocity can be computed using
$$
\begin{bmatrix}
\mathbf{C}(\Delta t_{k-1} \omega^{b_{k-1}w}) & \Delta t_{k-1} \mathbf{v}^{b_{k-1}a}_{b_{k-1}} \\
\mathbf{0} & 1
\end{bmatrix}
=
\mathbf{T}_{k-1}^{-1}\mathbf{T}_{k}.
$$

Note on computing arbitrary points

The kinematic motion model presented above may not be applicable for your application.
For example, if the point $b$ is not exactly on top of the robot wheel (for the $SE(2)$ example above), then the kinematic equation would probably be slightly different, so you'll need to derive the kinematic equation for that model.

Note on non-invertible motion models

If the motion model is not invertible, then perhaps using (nonlinear) least squares is your best bet.
Nonlinear least squares is the basis of some estimation algorithms such as the (extended) Kalman filter.
For example, this approach would be applicable if you're trying to estimate the angular velocity using position estimates from a GPS.
In such case, you need at least an initial guess of the heading. Otherwise, the problem would be non-observable.

Title: Estimating angular velocity from position

Body: If you have two position estimates $P_{t_1}$ and $P_{t_2}$ and times $t_1$ and $t_2$ how do you extract the average angular velocity from them? The linear velocity is a simple difference but how do you compute angular velocity?


问:
Create object that is invisible to the RangeFinder sensor
The title says it all. I would like to have object that the rangeFinder can't see. This would help me to model real life shiny objects for a ToF sensor in webots. Any suggestions?</p>

答:
You can set the visibility of nodes w.r.t. rendering devices (Camera, Lidar, RangeFinder or Viewpoint) using the Supervisor function wb_supervisor_node_set_visibility.

If your robot equipped with the RangeFinder has supervisor functionalities (i.e. the Robot.supervisor field is set to TRUE) you can simply set the objects visibility directly in the robot controller.
Otherwise, you might need to add an empty Robot node in your world, with supervisor field set to TRUE,  that sets the nodes visibility at the very beginning of the simulation.

Here is a sample python controller to set the visibility that retrieves both the object and RangeFinder nodes using the DEF name:

from controller import Supervisor

supervisor = Supervisor()
range_finder = supervisor.getFromDef("RANGE_FINDER")
hidden_object= supervisor.getFromDef("HIDDEN_OBJECT")
hidden_object.setVisibility(range_finder, False)
```

Disclaimer: I'm part of the Webots development team.

Body:
You can set the visibility of nodes w.r.t. rendering devices (Camera, Lidar, RangeFinder or Viewpoint) using the Supervisor function wb_supervisor_node_set_visibility.

If your robot equipped with the RangeFinder has supervisor functionalities (i.e. the Robot.supervisor field is set to TRUE) you can simply set the objects visibility directly in the robot controller.
Otherwise, you might need to add an empty Robot node in your world, with supervisor field set to TRUE,  that sets the nodes visibility at the very beginning of the simulation.

Here is a sample python controller to set the visibility that retrieves both the object and RangeFinder nodes using the DEF name:

from controller import Supervisor

supervisor = Supervisor()
range_finder = supervisor.getFromDef("RANGE_FINDER")
hidden_object= supervisor.getFromDef("HIDDEN_OBJECT")
hidden_object.setVisibility(range_finder, False)

Disclaimer: I'm part of the Webots development team.


问:
How to change the controller in Webots?

I'm rather new at Webots.

How do I change the controller for a node to another controller code in a different location?

I'm having trouble changing the code for an e-puck. The edit controller always takes me back to the original code, and I can't save to another location.

I saved it as Epuck_Glider.c, but it won't take any coding changes, and whenever I try to reopen it via "edit controller" it always reopens and runs the original code.

答:
The controller can be changed from the Field editor (located just below the Scene Tree).
When you select the EPuck.controller field, the Field editor displays the currently used controller as well a "Select..." button to change it:

Note that Webots expects that a controller is located in a directory having the same name as the main controller source file. So, if you save your controller C file with a different name in the same directory as another controller, this new C controller won't be recognized.
Here you can find some documentation about the expected Webots project directory structure.

If you want to create a new controller, then the simplest option is to use the "New Robot Controller..." wizard. Since Webots R2022b this wizard is located in the "File / New" menu.

Disclaimer: I am a part of the Webots development team.

Title: How to change the controller in Webots?


问:
Building a mobile camera platform
I have zero experience with robotics, but I need to build a mobile platform for a streaming camera. The idea is that I'll plug in my Android phone into the pan/tilt unit on my wheeled robot and then drive and look around via WiFi. I have already solved all of the software, interface and controller issues, but I would appreciate some advice on how to build the wheeled platform.

My initial idea was to buy a cheap RC car, remove all electronics and replace them with my own. This approach almost worked. I purchased this New Bright F-150 Truck.  The size is good and there is plenty of storage space:

However, I quickly ran into a problem with this thing. I assumed that the front wheel would be turned by some kind of servo. Instead I found this nonsense:

That small gear shaft is not driven by a servo - it's a conventional motor, which spins until it is jammed at the extremes of travel. The wheels are straightened when power is removed by a small spring on the other side. This means that there is only one angle at which the wheels can be turned, and that angle is way too small for what I need. So using this RC car will not work.

Before I start buying more things, I would like to hear some opinions from more experienced people. Am I on the right track? Do I simply need to get a better RC car, or are they all designed like this? Perhaps there are other options that would be more suitable for what I am doing?

答:
Depending on the strength of the spring, you may be able to convert the power to the steering motor to a PWM solution.  That would allow you to vary the overall voltage going to the motor, and (hopefully) allow you adjust the balance between the motor force and the spring force -- resulting in an adjustable steering angle.

This would be less preferable than a servo-based steering design, but might be a good last-ditch effort before upgrading to a more capable platform.

Title: Building a mobile camera platform


问:
Covariance of fused poses. Should it be normalised by the number of poses?
I came across this paper from T. Barfoot and P. Furgale:
"Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems"
Link: http://ncfrn.mcgill.ca/members/pubs/barfoot_tro14.pdf

where in Section IV.A is explained that if I have a list of $K$ measurements $x_k$ affected by zero-mean Gaussian noise $\{x_1, \Sigma_1\},\{x_2, \Sigma_2\}, ... ,\{x_K, \Sigma_K\}$ then the mean and covariance of the fusion of such measurements $\{\bar{x},\Sigma\}$ can be found in closed form:
\begin{equation}
\bar{x} = \Sigma \sum_{k=1}^K\Sigma_k^{-1}x_k,\quad\quad \Sigma = \Bigg(\sum_{k=1}^K\Sigma_k^{-1}\Bigg)^{-1}
\end{equation}

While the computation for the mean seems correct, I'm not totally convinced that $\Sigma$ is the true covariance of the fused measurements, because there is no normalisation against K, so the more measurements are present, the smaller the covariance will be.

Let's consider the simplest case where I have two identical scalar measurements with two identical (co)variance values
\begin{equation}
\{x_1 = 1, \Sigma_1 = 4\}, \{x_2 = 1, \Sigma_2 = 4\}
\end{equation}

In that case, the mean would be obviously $\bar{x} = 1$, but the covariance would be halved: $\Sigma = (\frac{1}{4}+\frac{1}{4})^{-1} = 2$.

Now, I understand that mere fact that there are two measurements instead of one gives a sort of confirmation that the true mean might be $1$, however I'd expect that the covariance would be still the same as the other two measurements, i.e. $\Sigma = \Sigma_1 = \Sigma_2$, because the second measurement is not bringing any additional information over the previous one.

答:
The short answer is no, there shouldn't be a normalizing constant, because the uncertainty should decrease as we get additional independent measurements.
I'll use an example to explain this mathematically and then I'll try to give some high-level understanding of why that's the case.

Simple example: Averaging random variables

Say we want to measure some quantity (e.g., room temperature) using a thermometer.
Furthermore, let's model the thermometer using a Gaussian random variable $\underline{y}$, where the error is given by $\underline{y} - x\sim\mathcal{N}(0, \sigma_{y}^{2})$, and $x$ is the true (unknown) temperature we're interested in measuring (note that I use underlines $\underline{(\cdot)}$ to denote a random variable).
Then each measurement $y_{i}\leftarrow \underline{y}$ is an independent realization/sample of this random variable.
The random variable can be rewritten as $\underline{y} - y_{i}\sim(0,\sigma_{y}^{2})$, which is useful for the derivation below.

To estimate the true temperature $x$, we need an estimator, which can be thought of as a function of the observations $y_{i}$ sampled from the random variables $\underline{y}_{i}$.
One possible estimator is the average estimator, given by
$$
\underline{\hat{x}} = \frac{1}{n}\sum_{i=1}^{n}\underline{y}_{i}.
$$
The estimate $\hat{x}$ is then the mean of the estimator $\underline{\hat{x}}$
$$
\hat{x}
=
\mathbb{E}[\underline{\hat{x}}]
=
\mathbb{E}\left[
   \frac{1}{n}\sum_{i=1}^{n}\underline{y}_{i}
\right]
=
\frac{1}{n}\sum_{i=1}^{n}\mathbb{E}[\underline{y}_{i}]
=
\frac{1}{n}\sum_{i=1}^{n}y_{i}
$$
where $\mathbb{E}[\cdot]$ is the expectation operator.
The variance on the estimator $\hat{\sigma}^{2}$ is then
$$
\hat{\sigma}^{2}
=
\operatorname{Var}[\underline{\hat{x}}]
=
\operatorname{Var}\left[
   \frac{1}{n}\sum_{i=1}^{n}\underline{y}_{i}
\right]
=
\frac{1}{n^{2}}\sum_{i=1}^{n}\operatorname{Var}[\underline{y}_{i}]
=
\frac{1}{n^{2}}\sum_{i=1}^{n}\sigma_{y}^{2},
$$
which uses the (co)variance rules and assumes measurements are independent.
Without this assumption, the result wouldn't hold.

Note that the covariance is independent of the measurements, and is rather only dependent on the measurements' covariance $\sigma_{y}^{2}$, which is usually a property of the sensor.
So, regardless of the sensor measurements (even if they're all identical), the covariance will decrease.
This property is specific to Gaussian random variables passed through linear functions (e.g., the averaging operator) and, in general, doesn't hold for other arbitrary estimators.

The above results are the same results you presented in your question.
I won't go into more details into about estimators and state-estimation, but more info can be found on these slides.
However, I'll try to give some insight into why the results make sense.

Why does the covariance decrease?

It may be counterintuitive that the estimate could have a smaller covariance than the measurements it's computing the estimate from.
However, the answer is somewhat related to the law of large numbers, where the more measurements you have, the closer you get to the true distribution.

This also makes sense intuitively, even in real life.
For example, say you read the room temperature from the wall thermometer, and you're not so convinced.
So you take another thermometer and see identical measurement.
Wouldn't that give you confidence that the measurement was true in the first place?
This feeling of confidence is a reduction in uncertainty.

I'd expect that the covariance would be still the same as the other two measurements, i.e. $\Sigma = \Sigma_{1} = \Sigma_{2}$, because the second measurement is not bringing any additional information over the previous one.

Actually, the mere presence of another independent measurement is additional information that reduces uncertainty.
It's important that the measurements are independent. Otherwise, the new measurements are correlated to some older measurements, which is uninformative (i.e., we already got similar information).


问:
Is it possible to increase computing power in a Lego Mindstorms v4 robot?
I was wondering if it is possible to increase the computing power in a Lego Mindstorms robot. For example by either being able to switch components on the mainboard or by completely exchanging the "original brain" with another device, e.g. a Raspberry PI or Arduino. As far as I am aware there is also a way to plug additional custom boards into the v4. Is it possible to attach an additional processing unit to some port and use it to execute calculations on the attached unit?

Unfortunately, I did not find anything about this on the internet other than for the EV3 version.

If that is possible, are there any manuals to do this out there or possibly even preassembled kits?

If nobody has ever done this, is it likely that this is possible by "hacking" ports and sending custom data to a custom-built unit? Since it has been done for the EV3 I would guess it is just a matter of time, right?

Thank you for any help

答: Yes, you can use a Raspberry Pi with Lego Mindstorms. I am making a Rubix Cube solving robot using a Dexter Industries BrickPi case and a Raspberry Pi Model B. That specific robot needs an older EV3 Mindstorms kit but I was able to buy all the components recently.</p>


问:
How can I draw in the 3D window in Webots?
I would like to draw some text or objects in the 3D view of Webots, how can I do that?</p>

答:
There are different techniques depending on what you want to draw:

If you just want to add some 2D text, you can do this by using the function: wb_supervisor_set_label. This will allow you to put 2D overlay text in front of the 3D simulation. Please lookup for the Supervisor API documentation.

If you want to add a small sub-window in front of the 3D graphics, you should consider using the Display node. This will allow you to do 2D vector graphics and text. This is also useful for example to display processed camera images. Please lookup for the Display node documentation.

If you want to add 3D graphics to the main window, this can be done by using the Supervisor API. The Supervisor API can be used to create new nodes - meaning that you can create an IndexedFaceSet or IndexedLineSet and adjust vertex positions or indexing accordingly to create and update the shape you wish to draw in the 3D scene.


问:
Trajectory generation for robotic manipulator using trapezoidal velocity profile in MATLAB
I am trying to write code in MATLAB to generate a trajectory for a scara manipulator in the robot operational space with trapezoidal velocity profile for each segment passing through 5 waypoints using the formulas shown in the attached image.

They are for creating a trajectory in operational space for each segment passing through multiple points. I am not sure how to implement $p_j - p_{j-1}$ since Matlab starts from index 1 and gives error on accessing previous index. Also how should one define $s'_j(t)$ for $t_{j-1} < t < t_j$ , for $t_0$ $s'_j(t) = 0$ and for $t_f$, $s'_j(t)$ becomes $||p_j - p_{j-1}||$ when the sampling time is 0.001 seconds.

If anybody has worked on trajectory planning and coded in MATLAB please can you help me with this, this image has been taken from one of the ppts shared by my Masters Professor but it can be found in the book -

Robotics: Modelling, Planning and Control by B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo, Chapter 4 Trajectory Planning. The book can be found on springer if anybody wants to check out, thank you

答:
I am not sure how to implement pj−pj−1 since Matlab starts from index 1 and gives error on accessing previous index.

You'll have some starting state, which would be p(1). Then, for trajectory planning, you would start at j = 2 and go from there. Remember that nearly every other programming language is a zero-based index, so when you see a term like p0 in your equation, that's p(1) in Matlab, and then j=1 in your summation would be j=2 in Matlab.


问:
Definition of Collision Frame and Inertial Frame in PyBullet or Other Simulation Software
I am doing simulation of my robot with PyBullet now, but I guess this question applies to other simulation software as well.

When I am using the functions createCollisionShape and createMultiBody, I am really confused about the definition of parameters like collisionFramePosition and linkInertialFramePositions. What is the definition of collision frame and inertial frame? The Quickstart Guide on the PyBullet website does not seem to have an explanation. Could someone tell me the definition or give me some resources to read?

答:
My original answer on this confused linkInertialFramePositions with baseInertialFramePosition, so here is a (hopefully ;) ) more accurate description of the two:

The linkInertialFramePositions parameter is used to define the "local" inertial frame of reference of the given link with respect to the link frame position. Thus, if the link frame is located at [0.5, 0.5, 0.5] at a given point in time, then the value of linkInertialFramePositions will act as an offset to [0.5, 0.5, 0.5]. This is important when considering the functions applyExternalForce/Torque as specifying flags=p.LINK_FRAME will have this force/torque acting with respect to the local inertial frame. So, Although perhaps an overly broad generalization, the link inertial frames define the origin of forces and torques acting on a body relative to the link frame.

The collisionFramePosition and visualFramePosition variables act similarly, but deal with moving the actual shapes around, nothing inherently connected to any forces/torques you might appply (although I would imagine this will effect how the body behaves with respect to forces/torques applied in the local frame). These values will act as a sort of offset to the link frame when added to a multibody system. In other words, if a link is added to a multibody system with the aforementioned variables both set to [0.1, 0.1, 0.1], the link will be rendered at an offset of [0.1, 0.1, 0.1] with respect to the link frame, and its collision shape will also be at the same offset with respect to the link frame. Now, if you would like to change the link frame position instead of working with these fixed offsets, createMultiBody has a flag linkPositions that can be used to change the position of a link with respect to its parent.


问:
wheels are spinning but robot doesn't move
I build a new Robot with Webots (two wheel cart + 1) and build a simple controller. I can see the wheels are rotating when I start the robot, but robot is not moving at all.

I am very new to Webots and I can't see relevant answer in the search result. Perhaps, I am missing something, but can't see it.

What am I missing?

Files for can be found here

Thanks

答:
The problem is the bounding objects defined for your Robot node.

Bounding objects are the physical objects used by the physics engine.
Currently these objects are not moving so they create friction that prevents the robot to move:

The bounding object for the two wheels should not be defined in the Robot node, but in the HingeJoint.endPoint Solid node so that they rotate.

As it is implemented in your model, the back wheel doesn't rotate but it is meant to slide on the ground. But the friction between this wheel and the ground is too high and prevents the robot to move.

Here are the changes you need to apply to your world file:

Change the type of the DEF back_wheel Transform node to a Solid node and move the bounding object definition of the back wheel from the Robot node to this new Solid node.

Remove completely all the nodes from the Robot.boundingObject field, because the wheels bounding objects are already defined in the `HingeJoint.endPoint Solid nodes.

Additionally, if you want to simulate a rotating back wheel then you should add some specific contact properties for it:

Add a ContactProperties node in WorldInfo and set the following values:

ContactProperties {
  material1 "rear wheel"
  material2 "default"
  coulombFriction [
    0
  ]
}
```

coulombFriction value set to "0" completely removes the friction, but you might want to adjust it to a different value.

In the new DEF back_wheel Solid node, set the contactMaterial field to "rear wheel"

Just for reference, you could also have a look at the built-in model of the NVIDIA JetBot robot that has a similar structure.

Disclaimer: I am a software developer at Webots.

Body:
The problem is the bounding objects defined for your Robot node.

Bounding objects are the physical objects used by the physics engine.
Currently these objects are not moving so they create friction that prevents the robot to move:

The bounding object for the two wheels should not be defined in the Robot node, but in the HingeJoint.endPoint Solid node so that they rotate.

As it is implemented in your model, the back wheel doesn't rotate but it is meant to slide on the ground. But the friction between this wheel and the ground is too high and prevents the robot to move.

Here are the changes you need to apply to your world file:

Change the type of the DEF back_wheel Transform node to a Solid node and move the bounding object definition of the back wheel from the Robot node to this new Solid node.

Remove completely all the nodes from the Robot.boundingObject field, because the wheels bounding objects are already defined in the `HingeJoint.endPoint Solid nodes.

Additionally, if you want to simulate a rotating back wheel then you should add some specific contact properties for it:

Add a ContactProperties node in WorldInfo and set the following values:

ContactProperties {
  material1 "rear wheel"
  material2 "default"
  coulombFriction [
    0
  ]
}

coulombFriction value set to "0" completely removes the friction, but you might want to adjust it to a different value.

In the new DEF back_wheel Solid node, set the contactMaterial field to "rear wheel"

Just for reference, you could also have a look at the built-in model of the NVIDIA JetBot robot that has a similar structure.

Disclaimer: I am a software developer at Webots.


问:
simulating transfer function with python
I've been trying for quite a while to simulate a transfer function using control library on python but I keep getting the wrong result, does anyone know the reason?

This is what I get:

This is what I want:

This is the transfer function I'm trying to simulate:

Code I used in python (I just assumed that value for K)

```<code>s = ctl.tf('s')
K = 180
G1 = (K) * (s+0.1) / s
G2 = (1) / ((s+1)*(s+2)*(s+10))
G3 = G1*G2
t = np.linspace(0, 20, 1000) #Tempo de 0s a 20s
_, y = ctl.impulse_response(G3, t) #calculando a resposta de G1
plt.plot(t,y)
plt.xlabel('Time in seconds')
plt.ylabel('Amplitude')
plt.title('Impulse response')
plt.grid()
</code>```

答:
I just did a quick MATLAB check, and maybe the problem you have is due to the fact that you simulated the Open-Loop response of the system. You did not include the Feedback part of the block diagram you have given, according to your code.

The response you desire to get is probably the Closed-Loop response of the same system. So you need to use the feedback function of the same library and check if you get the result you aim to.


答:
Expanding on kucar's excellent answer, you have not modeled the whole system. Looking at the block diagram reduction rules, follow item 4 "Eliminating a Feedback Loop." I'll abbreviate the diagram here:

So you need another step, G4. Since you're using negative feedback, you get

```<code>G4 = G3/(1+G3)
</code>```

which, with your code as-written, gives you the following plot:

Had to scratch my head for a moment as to why this doesn't look like the desired output in your question, and then I realized you're plotting the impulse response instead of the step response. The impulse response is as if you tapped the system with a hammer, and you're seeing the ringing response. The step response is what's shown in your desired output, where the input is a constant value (instead of an impulse).

Using _, y = ctl.step_response(G4, t) with K = 180 gives the following plot:

Title: simulating transfer function with python

Body:
I've been trying for quite a while to simulate a transfer function using control library on python but I keep getting the wrong result, does anyone know the reason?

This is what I get:

This is what I want:

This is the transfer function I'm trying to simulate:

Code I used in python (I just assumed that value for K)

```<code>s = ctl.tf('s')
K = 180
G1 = (K) * (s+0.1) / s
G2 = (1) / ((s+1)*(s+2)*(s+10))
G3 = G1*G2
t = np.linspace(0, 20, 1000) #Tempo de 0s a 20s
_, y = ctl.impulse_response(G3, t) #calculando a resposta de G1
plt.plot(t,y)
plt.xlabel('Time in seconds')
plt.ylabel('Amplitude')
plt.title('Impulse response')
plt.grid()
</code>```

Body:
Expanding on kucar's excellent answer, you have not modeled the whole system. Looking at the block diagram reduction rules, follow item 4 "Eliminating a Feedback Loop." I'll abbreviate the diagram here:

So you need another step, G4. Since you're using negative feedback, you get

G4 = G3/(1+G3)

which, with your code as-written, gives you the following plot:

Had to scratch my head for a moment as to why this doesn't look like the desired output in your question, and then I realized you're plotting the impulse response instead of the step response. The impulse response is as if you tapped the system with a hammer, and you're seeing the ringing response. The step response is what's shown in your desired output, where the input is a constant value (instead of an impulse).

Using _, y = ctl.step_response(G4, t) with K = 180 gives the following plot:


问:
Number of threads keep increasing in htop when running my python ROS node with 2 threads
I am writing a ROS node to read CAN data (linear x speed, and direction of motion i.e. rev or fwd) from my vehicle and my steer-by-wire system (encoder data for angular z). Both CAN interfaces are on separate CAN bus. I am using that data to generate odometry. We will call this node odom node.The odom node has 2 threads : one thread to generate linear velocity and another one to generate angular z velocity. The node also has a publisher for each of those parameters as well.

I also have a planning algorithm that works fine without the odom node, however, when I run my planning algorithm with my odom node, the planning algorithm crashes.

I opened up htop to see if its some sort of memory issue, I noticed that while the odom node is running, the number of threads keep on increasing (please refer to the image attached, 10740 threads).

I am not sure why the number of threads keep increasing, since Python has GIL, the first thread should get terminated before the second one is started. Could someone please help me figure out what am I doing wrong?

Code :

```<code>#!/usr/bin/env python3

import cantools
import can
from pprint import pprint
import rospy
from geometry_msgs.msg import Twist
import numpy as np
import fixedint
import std_msgs.msg
from std_msgs.msg import Float64
from nav_msgs.msg import Odometry
import math
import threading
import unittest

db = cantools.database.load_file('dbc_files/kia.dbc')
vehicle_can_bus = can.interface.Bus('can0', bustype='socketcan')
steer_by_wire_can_bus = can.interface.Bus('can1', bustype="socketcan")

class Telemetry:

    def __init__(self, linear_x, angular_z, encoder_angle,direction):

        self.linear_x = linear_x
        self.angular_z = angular_z
        self.encoder_angle = encoder_angle
        self.direction = direction

        try :

            self.linear_rate = rospy.get_param('~linear_vel_rate')
            self.angular_rate = rospy.get_param('~angular_vel_rate')

        except KeyError:
            print ("Could not set angular and linear thread rate parameters!!!")

        rospy.Timer(rospy.Duration(1.0 / self.linear_rate), self.linear_velocity_loop)
        rospy.Timer(rospy.Duration(1.0 / self.angular_rate), self.angular_velocity_loop)

    def linear_velocity_loop(self,event):

        while not rospy.is_shutdown() :

            # reader = can.BufferedReader()
            # notifier = can.Notifier(vehicle_can_bus, [reader], 10) # bus, listeners, time
            # message = reader.get_message(10) # timeout
            # if (message.arbitration_id == 213786525):
            #     rospy.loginfo_throttle((1/10),message.arbitration_id)

            message = vehicle_can_bus.recv()

            if message.arbitration_id == 317284680: # ID of ShtlDisp
                 # this message will print every 0.05 seconds (1/20)
                rospy.loginfo_throttle((1/10), "Shuttle msg")
                shuttle_data = db.decode_message('ShtlDisp', message.data)
                ShtlSymbol = shuttle_data['ShtlSymbol']

                if (ShtlSymbol == 3) :  # if the shuttle is in rev position, ShtlSymbol == 1 is fwd, 2 is neutral, 3 is rev
                    self.direction  = -1
                    # rospy.loginfo("The shuttle status is reverse")

                elif ShtlSymbol == 1 :
                    self.direction  = 1
                    # rospy.loginfo("The shuttle status is forward")

                # else:
                    # self.direction  = 0
                    # rospy.loginfo("The shuttle status is neutral")

            if message.arbitration_id == 213786525: # ID converted to HEX. Make sure if CAN ID is 19 or 29 bit. ID of DbmSpeedDisplay 213786525

                rospy.loginfo_throttle((1/10), "Received speed from CAN")

                readable_data = db.decode_message('DbmSpeedDisplay', message.data)
                SpeedDisplay_H = readable_data['SpeedDisplay_H']
                SpeedDecimalPoint = readable_data['SpeedDecimalPoint']
                SpeedDisplay_L = readable_data['SpeedDisplay_L'] # to kmph (dib by 100) and then m/sec (div 3.6)
                PtoRpm_H = readable_data['PtoRpm_H']
                PtoRpm_L = readable_data['PtoRpm_L']

                if (SpeedDisplay_H == 0):
                    if (SpeedDisplay_L < 0):
                        SpeedDisplay_L =  255 + SpeedDisplay_L

                if (SpeedDisplay_H == 1):
                    if (SpeedDisplay_L > 0):
                        SpeedDisplay_L =  255 + SpeedDisplay_L

                velocity = SpeedDisplay_L*self.direction
                self.linear_x = velocity/(100*3.6) # 3.6 to kmph >> m/s and div by 100 to chnage from 116 m/h (from CAN) to 1.16 kmph

            # while not reader.buffer.empty():
            #        reader.buffer.get()

    def angular_velocity_loop(self,event):

        while not rospy.is_shutdown() :

            dbw_reader = can.BufferedReader()
            dbw_notifier = can.Notifier(steer_by_wire_can_bus, [dbw_reader], 0.1) # bus, listeners, time
            encoder_message = dbw_reader.get_message(0.1) # timeout

            # encoder_message = steer_by_wire_can_bus.recv()

            if encoder_message and encoder_message.arbitration_id == 1:

                #rospy.loginfo_throttle (duration (n), message) # publish the message every n seconds

                rospy.loginfo_throttle((1/10), "Recieved Encoder Data") # this message will print every 0.05 seconds (1/20)

                encoder_data = np.uint16(encoder_message.data)

                xx = encoder_data[3] | (encoder_data[4] << 8)

                xx &= 0x0FFF

                if xx & 0x0800:
                    xx |= 0xF000

                angle = np.int16(xx) * (-360.0 / 4096.0)

                if angle > 0.0:
                    angle *= 1.21582733812

                angle =  angle *(3.14/180)

                self.encoder_angle = angle

                # angular speed (z) calculation
                wheelbase =  rospy.get_param('~wheelbase')
                self.angular_z = self.linear_x * math.tan(self.encoder_angle)/wheelbase

                # rospy.sleep(rospy.Duration(2))

                # clear CAN message buffer to always get the latest message

                while not dbw_reader.buffer.empty():
                   dbw_reader.buffer.get()

def telematics_publish(telemetry):

    pub = rospy.Publisher('/vehicle_telematics/odom', Odometry, queue_size=10)
    encoder_pub = rospy.Publisher('/vehicle_telematics/encoder', Float64, queue_size=10)

    try:
        pub_rate = rospy.get_param('~publishing_rate')
        rate = rospy.Rate(pub_rate)
    except KeyError:
        rospy.logerr("Could not set publishing rate !!!")

    # rate = rospy.Rate(10)

    twist = Twist()
    odom = Odometry()

    while not rospy.is_shutdown():

        odom.header.stamp = rospy.Time.now()
        odom.header.frame_id = 'base_odom'

        odom.pose.pose.position.x = 0
        odom.pose.pose.position.y = 0
        odom.pose.pose.position.z = 0.0

        odom.pose.pose.orientation.x = 0
        odom.pose.pose.orientation.y = 0
        odom.pose.pose.orientation.z = 0
        odom.pose.pose.orientation.w = 0

        odom.child_frame_id = "odom"
        odom.twist.twist.linear.x = telemetry.linear_x
        odom.twist.twist.linear.y = 0
        odom.twist.twist.angular.z = telemetry.angular_z

        # rospy.loginfo(odom)
        # rospy.loginfo(telemetry.encoder_angle)

        pub.publish(odom)
        rospy.loginfo_throttle((1/10), "Odom published")

        encoder_pub.publish(telemetry.encoder_angle)
        rospy.loginfo_throttle((1/10), "Encoder published") # if rospy.loginfo seems to slow the node down then change 1/rate to 1/10. 1/20 seems to work great for all rospy.loginfo in this code.
        rate.sleep()

if __name__ == '__main__':
    try:
        rospy.init_node('vehicle_telematics')

        tele = Telemetry(0,0,0,1)

        telematics_publish(tele)

    except rospy.ROSInterruptException:
        pass
</code>```

答:
I can't reproduce your problem due to unspecified dependencies and other steps needed.

But looking at you code, you appear to have loops that go infinity (while not rospy.is_shutdown()) being started periodically (rospy.Timer(rospy.Duration(1.0 / self.angular_rate), self.angular_velocity_loop))

Depending on what's in those loops it seems like it would potentially produce more and more threads.


问:
Principal approach to control force of an electrically driven friction based griper
I would like to build a gripper that can pick and place irregularly shaped objects, for example, vegetables. I'd like to grasp the object with enough force so that it does not slip form the jaws, but not too much, so that I don't damage the object or the gripper.

I'd like my gripper to be electrically driven, but I cannot find a single off the shelf actuator that would allow me to control the jaw position and the gripping force.

Question

Does it make sense to want to control the gripper by exerted force, and if so, what is a popular approach to do this that I could find an off the shelf component for?

My research

One approach I found is to exert the force via an extension spring. As illustrated in this video. I believe that this approach belongs to "tendon" family, and maybe something like this is used in this high end dexterous hand. But I can't any generic components that I could buy and use in my gripper.

Another approach is to exert the force via a rotary spring, and potentially localize the mechanism very closely to the motor. I'm not sure if that's exactly what is done here, but something to this end: (Force servo, f-servo). Both links point to very experimental solutions, and again, I can't find anything I could buy.

I skimmed the book "Robot Grippers" by Monkman et. all., did not find anything there. All the DYI projects I find on google use simple servo motors, which I don't understand how they won't break the first time you miscalculate something.

答:
Gripper or end effector design is still very much an open field of research. There are many different approaches, tendon driven, gear/linkage driven, over actuated, under actuated, spring loaded etc.

They are typically very customized for the individual tasks to which they are to be applied. You're unlikely to find a full design that works for your specific application. And staying away from the shopping question aspect of this.

In general there are not standalone actuators or other components for grippers as the space constraints are too tight to allow for standardized interfaces. And manufacturing volumes are too small to support a marketplace

All the DYI projects I find on google use simple servo motors, which I don't understand how they won't break the first time you miscalculate something.

The solution to this is to have various forms of compliance in your system which protect the actuator from over torque conditions. As your research has found this is often through the use of springs either extensional or rotary.

There are extreme examples such as the iRobot Hand with fingers that a baseball can be hit out of the gripper (video) and it doesn't break and use inherently compliant construction materials.

Does it make sense to want to control the gripper by exerted force, and if so, what is a popular approach to do this that I could find an off the shelf component for?

It does make sense to sense and control per the force applied. Again the volume is low enough that this is mostly a research topic for pressure sensitive surface treatments. There are many different approaches which support pressure sensitivity, proximity sensing, and shear sensing. All of which are potentially very valuable to the ability to control the gripper.


问:
How can I speed-up my Webots simulation?
My simulation is running correctly but is slow. How can I speed it up?

(I am part of the development team of Webots and we often get this question so I hope it will help some people!)

答:
There are several ways to increase the simulation speed:

Use the Run button. This button runs the simulation as fast as
possible using all the available CPU power. Otherwise, using the
Real-Time running mode, Webots may not use all the available CPU
power in order to slow down the simulation to match the real time.

Increase the value of WorldInfo.basicTimeStep. This field sets the
granularity of the physics simulation. With a higher
WorldInfo.basicTimeStep, the simulation becomes faster but less
accurate. With a lower WorldInfo.basicTimeStep, the simulation
becomes slower but more accurate. There is an additional
restriction: WorldInfo.basicTimeStep must be chosen such as to be
an integer divisor of the control step which is the value passed as
a parameter to the wb_robot_step (or equivalent) function.

Decrease the value of WorldInfo.FPS. This field represents the
maximum rate at which the 3D display of the main windows is
refreshed. With a lower value, the simulation becomes faster but
more flickering. With a higher value, the simulation becomes slower
but less flickering.

Try changing the value of WorldInfo.optimalThreadCount. This field
specifies how many threads are used to simulate the physics of the
world. Depending on the world you can get a better performance by
reducing or increasing this value. In general it is better to have a
low number of threads for simple worlds and a bigger number of
threads for complex worlds that include several robots physically
independent from each other.

Disable unnecessary shadows. Webots uses a lot of CPU/GPU power to
compute how and where the objects shadows are cast. But shadows are
irrelevant for most simulation unless they should explicitly be seen
by cameras. Unnecessary shadows can be disabled by unchecking the
castShadows field of light nodes: PointLight, SpotLight, or
DirectionalLight.

Simplify your simulation by removing unnecessary objects. In
particular, try to minimize the number of Physics nodes. Avoid
using Solid nodes when a Transform or a Shape node can be used
instead.

Simplify the bounding objects to increase the speed of the collision
detection. Replace complex primitives, like IndexedFaceSet,
Mesh, ElevationGrid or Cylinder by simpler primitives, like
Sphere, Capsule, Box and Plane. Avoid using a composition of
primitives (in a Group or a Transform) when a single primitive
would do the trick.

Set controller field to <none> for any Robot node that doesn't
need to be controlled, instead of using the <generic> controller.


问:
Difference between 3D Camera(using IR projection) and Stereo Camera?
I am currently busy with a final year project which requires me to track people walking through a doorway.
I initially thought this may be possible using a normal camera and using some motion detection functions given in OpenCV, I have however come to the conclusion that the the camera is mounted too low for this to work effectively.(Height shown in the image below)

I have now been looking into using a 3D camera or a stereo camera to try and get around this problem.

I have seen similar examples where a Kinect(from Xbox 360) has been used to generate a depth map which is then processed and used to do the tracking, this was however done from a higher vantage point, and I found that the minimum operating range of the Kinect is 0.5m.

From what I have found, the Kinect uses an IR projector and receiver to generate its depth map, and have been looking at the Orbbec Astra S which uses a similar system and has a minimum working distance of 0.3m.

My question now:

What exactly would the difference be between the depth maps produced by a 3D camera that uses an IR projector and receiver, and a stereo camera such as the DUO/ZED type options?

I am just looking for some insight from people that may have used these types of cameras before

On a side note, am i going about this the right way? or should i be looking into Time of Flight Cameras instead?

----EDIT----:

My goal is to count the people moving into and out of the train doorway. I began this using OpenCV, initially with a background subtraction and blob detection method. This only worked for one person at a time and with a test video filmed at a higher vantage point as a "blob-merging" problem was encountered as shown in the left image below.

So the next method tested involved an optical flow method using motion vectors obtained from OpenCV's dense optical flow algorithm.
From which i was able to obtain motion vectors from the higher test videos and track them as shown in the middle image below, because of the densely packed and easily detected motion vectors it was simple to cluster them.

But when this same system was attempted with footage taken from inside a train at a lower height, it was unable to give a consistant output. My thoughts of the reasons for this was because of the low height of the camera, single camera tracking is able to function when there is sufficient space between the camera and the top of the person. But as the distance is minimized, the area of the frame that the moving person takes up becomes larger and larger, and the space to which the person can be compared is reduced (or atleast that is how I understand it). Below on the right you can see how in the image the color of the persons clothing is almost uniform, Optical flow is therefore unable to detect it as motion in both cases.

I only started working with computer vision a few months ago so please forgive me if I have missed some crucial aspects.

From what i have seen from research, most commercial systems make used of a 3D cameras, stereo cameras or Time-of-Flight cameras, but I am unsure as to how the specifics of each of these would be best suited for my application.

答:
With stereo cameras, we are still dependent of having features/texture in the scene, regardless of using sparce or dense optical flow.

The depth is a function of the baseline, but we can have two cameras and control the baseline needed for the application.

So, I would say that the benefits are that we can select the resolution of the cameras, the frame rate and the field of view of the lens.

The system used by Kinect is widely available, for example in phones. Although is often not easy to access raw data.

But we can still do stereo with those systems if we have access to raw data. The stereo here is limited because typically the infrared camera has a lower resolution compared to the RGB camera and some systems have a wide angle RGB.

The advantage is that we have N-thousand points with depth that are in reflective surfaces and are not dependent of having texture.

Also, the IR depth is around 1m to 5m.

But I would use a fisheye in this application ;)

With this we would get early detecting and tracking.

The distortion would separate the heads from bodies, and it would facilitate separating people.

And we can get depth by measuring the motion with respect to the ground (planar induced homography).


问:
SLAM with multiple cameras and asynchronous sensors
I'm pretty sure this is the right place for this question, but if not, please point me in the right direction!

Basically, I would like to know how do SLAM using multiple cameras and sensors that are asynchronous. Along with this, I'm curious if there's any way to use the SLAM information—or any way in general—to automatically align the pose data from multiple devices into the same coordinate system.

My specific situation is that I have devices that each have 2 cameras, 2 IMUs without built-in magnetometers, a magnetometer, and a GPS. The cameras use fisheye lenses, point different directions (but have a little overlap on their images), and capture images at the same time. However, all other sensors take measurements at different times. The IMUs have similar capture rates but don't capture at the same time, the magnetometer takes measurements much less often that the IMUs do, etc. I have the transformation matrices to convert between the sensors' local coordinate systems, along with other intrinsic and extrinsic data.

I'd like to get the orientations and positions from all of this data as well as a mapping of the environment. I also have recordings of the devices in the same location, so I want to put the devices' poses in the same reference frame so that they can be visualized at the same time. The algorithm doesn't have to do online / real-time SLAM, since I'm only analyzing the data after it's all been captured. Furthermore, it's not necessary for the algorithm to use all of the sensors, but I would like it to at least use both cameras. If possible, I'd also prefer the algorithm to not rely on ROS.

Most of the algorithms I found online didn't match my case exactly, and the one that I think might've doesn't have a public implementation that I can use.

答:
I’m making assumptions from the question … I guess the system has two phones fixed with duct tape ;)

So, I would calibrate sensors to the camera and calibrate camera to camera.

I would have visual odometry per camera and fuse with each sensor (with Kalman filter).

Get 3D with SFM and use bundle adjustment to minimize the errors in the camera to camera pose.

It sounds easy but this is a PhD thesis.

https://publikationen.bibliothek.kit.edu/1000080527/20396421[3D Visual Perception for Self-Driving Cars using a Multi-Camera System: Calibration, Mapping, Localization, and Obstacle Detection]


问:
Question about getting global coordinates of lidar point cloud from relative in Webots
I need to do custom mapping of surroundings with lidar using mobile robot in Webots. What I use for that:

GPS for getting robot position.

Compass for getting direction robot.

Lidar for getting info about surroundings.

I did translation and rotation of relative points from lidar, which worked well when robot is on flat surface.

But no matter how much I tried I can't figure out how to get accurate global coordinates from point cloud relative points, when robot is even a bit tilted.

My guess is that it suppose to use spatial transformation matrices, but I not sure how to use Webots Compass values in rotation matrix.

Maybe someone can show code example or explain the math behind it or there is a method that I missed in Webots?

答:
I guess you’re using GPS to get the initial transformation, matching lidar points with ICP and re-calculating the pose update.

P’1 = Pose_gps * P1

Match P2 with P’1 using ICP

Pose = P2 * pinv(P1)

I would use a Kalman filter with the pose in state vector and use the motion model in the state machine to constraint the motion.

Don’t forget to use quaternions and avoid gimble lock ;)