Skip to content

Commit bc7f8cf

Browse files
Cionix90Cionix90
authored
Merge pull request #52 from CentroEPiaggio/master
Update of ROS2 Pronto estimator
2 parents cfc9074 + d3ed9f3 commit bc7f8cf

59 files changed

Lines changed: 350 additions & 7391 deletions

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

‎.gitignore‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,4 +4,5 @@ CMakeLists.txt.user
44
/install/
55
/log/
66
/build/
7-
pronto_ros2_node/Experiments/
7+
pronto_ros2_node/Experiments/
8+
src/example/pronto_config/bags/

‎DockerFiles/Dockerfile‎

Lines changed: 84 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,84 @@
1+
2+
# base image osrf/ros tag humble-desktop-full
3+
ARG BASE_IMAGE=osrf/ros
4+
ARG BASE_TAG=humble-desktop-full
5+
FROM ${BASE_IMAGE}:${BASE_TAG}
6+
7+
# set the environment variable with the command ENV <key>=<value>, it can be replaced online
8+
ENV DEBIAN_FRONTEND=noninteractive
9+
10+
# RUN is used to execute and add new layer on top of the base immage
11+
12+
RUN apt-get update \
13+
&& apt-get install -y \
14+
ros-humble-gazebo-ros-pkgs \
15+
ros-humble-gazebo-ros2-control \
16+
ros-humble-joint-state-publisher \
17+
ros-humble-joint-state-publisher-gui \
18+
ros-humble-pinocchio \
19+
ros-humble-ros2-control \
20+
ros-humble-ros2-controllers \
21+
ros-humble-xacro \
22+
ros-humble-rosbag2-storage-mcap \
23+
ros-humble-plotjuggler-ros \
24+
chrony \
25+
tmux python3-pip\
26+
xterm \
27+
libeigen3-dev \
28+
nano \
29+
ros-humble-rviz2 \
30+
nautilus \
31+
iputils-ping \
32+
iproute2 \
33+
python3-vcstool
34+
# Adapt your desired python version here
35+
# ENV PATH=/opt/openrobots/bin:$PATH
36+
# ENV PKG_CONFIG_PATH=/opt/openrobots/lib/pkgconfig:$PKG_CONFIG_PATH
37+
# ENV LD_LIBRARY_PATH=/opt/openrobots/lib:$LD_LIBRARY_PATH
38+
# ENV PYTHONPATH=/opt/openrobots/lib/python3.10/site-packages:$PYTHONPATH
39+
# ENV CMAKE_PREFIX_PATH=/opt/openrobots:$CMAKE_PREFIX_PATH
40+
# ENV TERM=xterm-256color
41+
42+
ENV DEBIAN_FRONTEND=dialog
43+
44+
# Create a new user
45+
ARG USERNAME=ros
46+
ARG USER_UID=1000
47+
ARG USER_GID=${USER_UID}
48+
RUN groupadd --gid ${USER_GID} ${USERNAME} \
49+
&& useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} \
50+
&& apt-get update \
51+
&& apt-get install -y sudo \
52+
&& echo ${USERNAME} ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/${USERNAME} \
53+
&& chmod 0440 /etc/sudoers.d/${USERNAME}
54+
55+
#Change HOME environment variable
56+
ENV HOME /home/${USERNAME}
57+
58+
# Choose to run as user
59+
ENV USER ${USERNAME}
60+
61+
USER ${USERNAME}
62+
63+
# ********************************************************
64+
# * Anything else you want to do like clean up goes here *
65+
# ********************************************************
66+
67+
# Install the python packages cosi non vengono installati da root
68+
RUN pip3 install \
69+
numpy \
70+
numpy-quaternion \
71+
quadprog \
72+
scipy \
73+
--upgrade
74+
# Set up auto-source of workspace for ros user
75+
ARG WORKSPACE=docker_pronto_ws
76+
77+
RUN echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
78+
RUN echo 'source /usr/share/gazebo/setup.bash' >> ~/.bashrc
79+
RUN echo "if [ -f ~/${WORKSPACE}/install/setup.bash ]; then source ~/${WORKSPACE}/install/setup.bash; fi" >> /home/ros/.bashrc
80+
81+
WORKDIR /home/ros/${WORKSPACE}
82+
RUN mkdir src
83+
84+
ENTRYPOINT ["/ros_entrypoint.sh"]

‎compose.yaml‎

Lines changed: 57 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,57 @@
1+
services:
2+
pronto_ros2_humble:
3+
build:
4+
context: ./DockerFiles
5+
dockerfile: Dockerfile
6+
args:
7+
- WORKSPACE=docker_pronto_ws
8+
9+
container_name: pronto_docker
10+
network_mode: "host"
11+
# Interactive shell settings
12+
stdin_open: true
13+
tty: true
14+
# Network settings
15+
16+
# Environment variables
17+
environment:
18+
- DISPLAY=${DISPLAY}
19+
- QT_X11_NO_MITSHM=1
20+
- XAUTHORITY=/tmp/.docker.xauth
21+
- ROS_DOMAIN_ID=0
22+
# Shell history
23+
- HISTFILE=/home/ros/.bash_history
24+
- HISTFILESIZE=10000
25+
- RCUTILS_COLORIZED_OUTPUT=1
26+
privileged: true # Fixed spelling and removed duplicate
27+
volumes:
28+
- /tmp/.X11-unix:/tmp/.X11-unix:rw
29+
# - /tmp/.docker.xauth:/tmp/.docker.xauth:rw
30+
- ~/.bash_history:/home/ros/.bash_history
31+
- .:/home/ros/docker_pronto_ws/src
32+
- /dev/nvidia0:/dev/nvidia0
33+
- /dev/nvidiactl:/dev/nvidiactl
34+
- /dev/nvidia-modeset:/dev/nvidia-modeset
35+
- /dev:/dev
36+
37+
38+
39+
40+
# Command to source ROS2 environment and start shell
41+
command: >
42+
bash -c "
43+
sudo apt update --fix-missing &&
44+
source /opt/ros/humble/setup.bash &&
45+
if [ ! -f /etc/ros/rosdep/sources.list.d/20-default.list ]; then \
46+
sudo rosdep init \
47+
fi &&
48+
vcs import < ~/docker_pronto_ws/src/ros2_interfaces_repository.repos &&
49+
rosdep update &&
50+
echo 'ROS2 humble environment loaded' &&
51+
bash
52+
"
53+
54+
55+
56+
57+

‎pronto_quadruped/src/ImuBiasLock.cpp‎

Lines changed: 1 addition & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -37,19 +37,15 @@ ImuBiasLock::ImuBiasLock(const Eigen::Isometry3d& ins_to_body,
3737
// accel bias indices
3838
z_indices.block<3,1>(3,0) = RBIS::accelBiasInds();
3939
// roll and pitch indices
40-
z_indices.tail<2>(0) << RBIS::chi_ind, RBIS::chi_ind+1;
40+
z_indices.tail<2>() << RBIS::chi_ind, RBIS::chi_ind+1;
4141
gravity_vector_ = Eigen::Vector3d::UnitZ() * g_val;
42-
4342
z_covariance = CovMatrix::Zero();
44-
4543
eps_ = cfg.velocity_threshold_;
4644
torque_threshold_ = cfg.torque_threshold_;
4745
dt_ = cfg.dt_;
4846
debug_ = cfg.verbose_;
49-
5047
bias_transform_ = Eigen::Isometry3d::Identity();
5148
gravity_transform_ = Eigen::Isometry3d::Identity();
52-
5349
gyro_bias_history_.reserve(max_size + 1);
5450
accel_bias_history_.reserve(max_size + 1);
5551
}

‎pronto_quadruped/src/LegOdometer.cpp‎

Lines changed: 4 additions & 71 deletions
Original file line numberDiff line numberDiff line change
@@ -246,55 +246,17 @@ bool LegOdometer::estimateVelocity(const uint64_t utime,
246246
} else if(a_mode_ == AverageMode::SIMPLE_AVG){
247247
// Computing average velocity
248248
for(int i = 0; i < 4; i++) {
249-
// std::cerr<<" the stance leg "<<i<<"-th leg is "<< stance_legs[i]<<std::endl;
249+
250250
if(stance_legs[i]) {
251251
xd_b_ += base_vel_leg_[i];
252252
leg_count++;
253253

254254
}
255-
// std::cerr<<"iteration is "<<i<<std::endl;
256-
257-
// std::cerr<<" xd_b_ is "<< xd_b_ << std::endl;
255+
258256
}
259-
// if(leg_count != 0)
260-
// std::cerr<<" leg count is "<< leg_count<<std::endl;
261-
262-
263-
// if(leg_count == 1 || leg_count == 3) {
264-
// xd_b_ = old_xd_b;
265-
// }
266-
// std::cerr<< "the leg count is "<< leg_count<<std::endl;
267-
// if(leg_count != 2 && leg_count != 4)
268-
// {
269-
// std::cerr<<" the old leg count is "<<old_leg_count<<std::endl;
270-
// if(old_leg_count == 2 || old_leg_count == 4)
271-
// {
272-
// xd_b_peak = old_xd_b;
273-
// std::cerr<<" x_peak has been updated : "<<xd_b_peak<<std::endl;
274-
// }
275-
// xd_b_ = xd_b_peak;
276-
// }
277-
278-
// std::cerr << stance_legs << std::endl << stance_legs[0] + stance_legs[1] + stance_legs[2] + stance_legs[3]<< std::endl;
279-
280-
// std::cerr<< "PDPDPPDPDPDPPDPDPD"<<std::endl;
281-
// if(stance_legs[0] + stance_legs[1] + stance_legs[2] + stance_legs[3] == 4)
282-
// {
283-
// std::cerr << "leg count è uno devo tornare false PDPDPDPDPD"<<std::endl;
284-
285-
// }
257+
286258
if(leg_count == 0) {
287-
// if(leg_count == 1)
288-
// {
289-
// std::cerr<<"at time "
290-
// }
291-
292-
// if(count == 0)
293-
// {
294-
// xd_b_peak = old_xd_b;
295-
// count ++;
296-
// }
297-
// xd_b_ = xd_b_peak;
259+
//
298260
return false;
299261
}
300262

@@ -312,35 +274,6 @@ bool LegOdometer::estimateVelocity(const uint64_t utime,
312274

313275

314276

315-
// mov_ave = xd_b_;
316-
// update_moving_mean(mov_ave);
317-
// // xd_b_ = mov_ave;
318-
// // std::cerr << " average value is "<< mov_ave(0)<<" meanwhile xb value is "<< xd_b_(0)<<" mean substitution index "<< claro<< std::endl<<std::endl;
319-
// if(std::abs(xd_b_(0) - old_xd_b(0)) > 0.01)
320-
// {
321-
// if(count == 0)
322-
// {
323-
// mov_ave_peak = mov_ave;
324-
// std::cerr<< "update peak average"<< mov_ave_peak.norm() <<std::endl;
325-
// count ++;
326-
// }
327-
// // if((old_leg_count == 2 || old_leg_count == 4))
328-
// // {
329-
// // xd_b_peak = old_xd_b;
330-
// // }
331-
// // std::cerr<< "hold the old correction "<< claro<< " act value.norm: "<<xd_b_.norm()<< " old value: "<<old_xd_b.norm() <<std::endl;
332-
333-
// xd_b_ = mov_ave_peak;
334-
// claro++;
335-
// }
336-
// else
337-
// count = 0;
338-
// std::cerr<< "hold the old correction "<< claro<< " act value.norm: "<<xd_b_.norm()<< " old value: "<<old_xd_b.norm() <<std::endl;
339-
340-
// if(xd_b_.norm() > 0.05)
341-
// {
342-
343-
// }
344277
if(xd_b_.norm() > 10){
345278
std::cerr << "+++++++++++++++++++++ABNORMAL VELOCITY: " << std::endl;
346279
Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]");

‎pronto_quadruped_ros/include/pronto_quadruped_ros/bias_lock_handler_ros.hpp‎

Lines changed: 19 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -52,7 +52,7 @@ class ImuBiasLockBaseROS : public DualSensingModule<sensor_msgs::msg::Imu, Joint
5252
{
5353
public:
5454
ImuBiasLockBaseROS() = delete;
55-
ImuBiasLockBaseROS(rclcpp::Node::SharedPtr nh);
55+
ImuBiasLockBaseROS(rclcpp::Node::SharedPtr nh,Eigen::Isometry3d ins_to_body);
5656
RBISUpdateInterface* processMessage(const sensor_msgs::msg::Imu *msg, StateEstimator *est) /*override*/;
5757
// tf2_ros::Buffer tfBuffer;
5858

@@ -86,7 +86,8 @@ class ImuBiasLockBaseROS : public DualSensingModule<sensor_msgs::msg::Imu, Joint
8686
};
8787

8888
template <class JointStateT>
89-
ImuBiasLockBaseROS<JointStateT>::ImuBiasLockBaseROS(rclcpp::Node::SharedPtr nh) : nh_(nh)
89+
ImuBiasLockBaseROS<JointStateT>::ImuBiasLockBaseROS(rclcpp::Node::SharedPtr nh,
90+
Eigen::Isometry3d ins_to_body) : nh_(nh)
9091
{
9192
tf2_ros::Buffer tfBuffer(nh_->get_clock());
9293
tf2_ros::TransformListener tf_imu_to_body_listener_(tfBuffer);
@@ -108,19 +109,19 @@ ImuBiasLockBaseROS<JointStateT>::ImuBiasLockBaseROS(rclcpp::Node::SharedPtr nh)
108109
std::string base_frame = "base";
109110
nh_->get_parameter_or<std::string>(ins_param_prefix + "base_link_name", base_frame, "base");
110111
RCLCPP_INFO_STREAM(nh_->get_logger(), "[ImuBiasLockBaseROS] Name of base_link: '" << base_frame << "'");
111-
Eigen::Isometry3d ins_to_body = Eigen::Isometry3d::Identity();
112-
while (rclcpp::ok()) {
113-
try {
114-
// lookupTransform API is : target_frame, source_frame
115-
geometry_msgs::msg::TransformStamped temp_transform = tfBuffer.lookupTransform(base_frame, imu_frame, tf2::TimePointZero);
116-
ins_to_body = tf2::transformToEigen(temp_transform.transform);
117-
RCLCPP_INFO_STREAM(nh_->get_logger(), "IMU (" << imu_frame << ") to base (" << base_frame << ") transform: translation=(" << ins_to_body.translation().transpose() << "), rotation=(" << ins_to_body.rotation() << ")");
118-
break;
119-
} catch (const tf2::TransformException& ex) {
120-
RCLCPP_ERROR(nh_->get_logger(), "%s", ex.what());
121-
rclcpp::sleep_for(std::chrono::seconds(1));
122-
}
123-
}
112+
// Eigen::Isometry3d ins_to_body = Eigen::Isometry3d::Identity();
113+
// while (rclcpp::ok()) {
114+
// try {
115+
// // lookupTransform API is : target_frame, source_frame
116+
// geometry_msgs::msg::TransformStamped temp_transform = tfBuffer.lookupTransform(base_frame, imu_frame, tf2::TimePointZero);
117+
// ins_to_body = tf2::transformToEigen(temp_transform.transform);
118+
// RCLCPP_INFO_STREAM(nh_->get_logger(), "IMU (" << imu_frame << ") to base (" << base_frame << ") transform: translation=(" << ins_to_body.translation().transpose() << "), rotation=(" << ins_to_body.rotation() << ")");
119+
// break;
120+
// } catch (const tf2::TransformException& ex) {
121+
// RCLCPP_ERROR(nh_->get_logger(), "%s", ex.what());
122+
// rclcpp::sleep_for(std::chrono::seconds(1));
123+
// }
124+
// }
124125

125126
quadruped::ImuBiasLockConfig cfg;
126127
nh_->get_parameter_or(lock_param_prefix + "torque_threshold", cfg.torque_threshold_, 0.0);
@@ -165,7 +166,6 @@ ImuBiasLockBaseROS<JointStateT>::ImuBiasLockBaseROS(rclcpp::Node::SharedPtr nh)
165166
base_arrow_.color.r = 0;
166167
base_arrow_.color.g = 1;
167168
}
168-
169169
bias_lock_module_ = std::make_unique<quadruped::ImuBiasLock>(ins_to_body, cfg);
170170
}
171171

@@ -268,7 +268,7 @@ bool ImuBiasLockBaseROS<JointStateT>::processMessageInit(
268268
class ImuBiasLockROS_Sim : public ImuBiasLockBaseROS<sensor_msgs::msg::JointState>
269269
{
270270
public:
271-
ImuBiasLockROS_Sim(rclcpp::Node::SharedPtr nh);
271+
ImuBiasLockROS_Sim(rclcpp::Node::SharedPtr nh, Eigen::Isometry3d ins_to_body);
272272
virtual ~ImuBiasLockROS_Sim() = default;
273273

274274
RBISUpdateInterface* processMessage(const sensor_msgs::msg::Imu *msg, StateEstimator *est) /*override*/;
@@ -288,7 +288,7 @@ class ImuBiasLockROS_Sim : public ImuBiasLockBaseROS<sensor_msgs::msg::JointStat
288288
class ImuBiasLockWithAccelerationROS : public ImuBiasLockBaseROS<pronto_msgs::msg::JointStateWithAcceleration>
289289
{
290290
public:
291-
ImuBiasLockWithAccelerationROS(rclcpp::Node::SharedPtr nh) : ImuBiasLockBaseROS<pronto_msgs::msg::JointStateWithAcceleration>(nh) {}
291+
ImuBiasLockWithAccelerationROS(rclcpp::Node::SharedPtr nh, Eigen::Isometry3d ins_to_body) : ImuBiasLockBaseROS<pronto_msgs::msg::JointStateWithAcceleration>(nh,ins_to_body) {}
292292
virtual ~ImuBiasLockWithAccelerationROS() = default;
293293

294294
void processSecondaryMessage(const pronto_msgs::msg::JointStateWithAcceleration &msg);
@@ -297,7 +297,7 @@ class ImuBiasLockWithAccelerationROS : public ImuBiasLockBaseROS<pronto_msgs::ms
297297
class ImuBiasLockROS: public ImuBiasLockBaseROS<pi3hat_moteus_int_msgs::msg::JointsStates>
298298
{
299299
public:
300-
ImuBiasLockROS(rclcpp::Node::SharedPtr nh);
300+
ImuBiasLockROS(rclcpp::Node::SharedPtr nh, Eigen::Isometry3d ins_to_body);
301301
virtual ~ImuBiasLockROS() = default;
302302

303303
RBISUpdateInterface* processMessage(const sensor_msgs::msg::Imu *msg, StateEstimator *est) /*override*/;

‎pronto_quadruped_ros/include/pronto_quadruped_ros/quad_model_parser.hpp‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
#include "urdf/model.h"
2-
2+
#include <Eigen/Core>
3+
#include <Eigen/Dense>
34

45
#include <string>
56
#include <vector>
@@ -43,6 +44,8 @@ namespace pronto
4344
~Model_Parser(){};
4445

4546
Axis_Direction get_ker_dir();
47+
48+
void get_imu_base_tranform(std::string base_link, std::string imu_link, Eigen::Isometry3d& tranformation);
4649

4750
int get_robot_DOF()
4851
{
@@ -64,6 +67,8 @@ namespace pronto
6467
};
6568

6669

70+
71+
6772
protected:
6873

6974
void get_jnt_list()

0 commit comments

Comments
 (0)