From b901ed40ddd9b6a9a75e7797f6dcd94583c5f481 Mon Sep 17 00:00:00 2001 From: jzhao6x <88374403+jzhao6x@users.noreply.github.com> Date: Wed, 16 Apr 2025 17:38:12 +0800 Subject: [PATCH 01/42] modify description name (#1) --- .../ocs2_legged_robot_ros/launch/legged_robot_ddp.launch | 2 +- .../ocs2_legged_robot_ros/launch/legged_robot_ipm.launch | 2 +- .../ocs2_legged_robot_ros/launch/legged_robot_sqp.launch | 2 +- .../ocs2_legged_robot_ros/rviz/legged_robot.rviz | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch index d0d0f7611..4ccf50083 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch @@ -3,7 +3,7 @@ - + diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch index 085a29175..59ab4ef4b 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch @@ -3,7 +3,7 @@ - + diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch index 8e11f2138..13d1e85ed 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch @@ -3,7 +3,7 @@ - + diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/rviz/legged_robot.rviz b/ocs2_robotic_examples/ocs2_legged_robot_ros/rviz/legged_robot.rviz index f119a5ee4..cf25c35fb 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/rviz/legged_robot.rviz +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/rviz/legged_robot.rviz @@ -427,7 +427,7 @@ Visualization Manager: Show Axes: false Show Trail: false Name: RobotModel - Robot Description: legged_robot_description + Robot Description: robot_description TF Prefix: "" Update Interval: 0 Value: true From 71e2381f374e132a46b6c597cf7953ded8a1c04b Mon Sep 17 00:00:00 2001 From: jzhao6x <88374403+jzhao6x@users.noreply.github.com> Date: Wed, 7 May 2025 10:17:31 +0800 Subject: [PATCH 02/42] modify for ocs2 (#2) * modify description name * add scripts to control robot --- .../scripts/legged_robot_control.py | 136 ++++++++++++++++++ .../scripts/franka_control.py | 111 ++++++++++++++ .../scripts/ridgeback_control.py | 107 ++++++++++++++ 3 files changed, 354 insertions(+) create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_ros/scripts/legged_robot_control.py create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/scripts/franka_control.py create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/scripts/ridgeback_control.py diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/scripts/legged_robot_control.py b/ocs2_robotic_examples/ocs2_legged_robot_ros/scripts/legged_robot_control.py new file mode 100644 index 000000000..a9b21a69d --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/scripts/legged_robot_control.py @@ -0,0 +1,136 @@ +#!/usr/bin/env python3 +import subprocess +import time +import rospy +import math +from tf2_msgs.msg import TFMessage +from functools import partial + + +target_distance_1 = (1.0, 1.0, 0.0, 0.0) # movement distance (x, y, z, yaw) +target_distance_2 = (-1.0, -1.0, 0.0, 0.0) +initial_translation = (0, 0, 0) +translation = None +tolerance = 0.01 +max_iteration = 10 + + +def set_gait(): + command = 'trot' + '\n' + process = subprocess.Popen( + ['rosrun', 'ocs2_legged_robot_ros', 'legged_robot_gait_command'], + stdin=subprocess.PIPE, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + text=True + ) + time.sleep(2) + + process.stdin.write(command) + process.stdin.flush() + + output = process.stdout.readline() + print(output.strip()) + +def run_legged_robot_target(target_distance): + try: + process = subprocess.Popen( + ['rosrun', 'ocs2_legged_robot_ros', 'legged_robot_target'], + stdin=subprocess.PIPE, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + text=True + ) + + # wait process to start + time.sleep(1) + + # Input XYZ and Yaw displacements + input_target = ' '.join(map(str, target_distance)) + # print(input_target) + process.stdin.write(input_target + '\n') + process.stdin.flush() + + # To wait command published + while True: + output = process.stdout.readline() + if output: + # print(output.strip() + '\n') + if "The following command is published" in output: + rospy.loginfo("Detected the target command published. Input: " + input_target) + break + + except Exception as e: + print(f"An error occurred: {e}") + finally: + process.terminate() + + +class TFListener: + def __init__(self, target_distance): + self.target_distance = target_distance + self.node_name = 'base_frame_listener' + self.subscriber = None + self.iteration = 1 + + def start(self): + rospy.init_node(self.node_name, anonymous=True) + self.subscriber = rospy.Subscriber('/tf', TFMessage, partial(self.callback, target_distance=self.target_distance)) + rospy.spin() + + def stop(self): + if self.subscriber: + self.subscriber.unregister() + self.subscriber = None + rospy.signal_shutdown('Listener stopped') + + def calculate_distance(self, trans1, trans2): + """Calculate two translations distance""" + dx = trans2[0] - trans1[0] + dy = trans2[1] - trans1[1] + return math.sqrt(dx**2 + dy**2) + + def callback(self, data, target_distance): + global translation, initial_translation + for transform in data.transforms: + if transform.child_frame_id == "base": + translation = ( + transform.transform.translation.x, + transform.transform.translation.y, + transform.transform.translation.z + ) + + # calculate two translations distance + distance = self.calculate_distance(initial_translation, translation) + target_distance_xy = math.sqrt(target_distance[0]**2 + target_distance[1]**2) + + if abs(distance - target_distance_xy) <= tolerance: + + rospy.loginfo("Target reached within tolerance. Translation: x=%f, y=%f, z=%f", + translation[0], + translation[1], + translation[2]) + initial_translation = translation # update initial translation for next movement + rospy.loginfo('movement ' + str(self.iteration)) + if self.iteration % 2 == 0: + target_distance = target_distance_1 + else: + target_distance = target_distance_2 + run_legged_robot_target(target_distance) + self.iteration += 1 + if self.iteration > max_iteration: + rospy.signal_shutdown("Target reached") + + +def main(): + # set gait to trot and wait a while to take effect + set_gait() + time.sleep(10) + run_legged_robot_target(target_distance_1) + listener = TFListener(target_distance_1) + listener.start() + + +if __name__ == "__main__": + main() + diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/scripts/franka_control.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/scripts/franka_control.py new file mode 100644 index 000000000..3fa3f028d --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/scripts/franka_control.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python + +import rospy +import tf +import time +from ocs2_msgs.msg import mpc_target_trajectories, mpc_state, mpc_input +from geometry_msgs.msg import Pose +from tf2_msgs.msg import TFMessage + +max_iteration = 10 + +class MobileManipulatorNode: + def __init__(self): + rospy.init_node('mobile_manipulator_franka_control_node', anonymous=True) + self.publisher = rospy.Publisher('/mobile_manipulator_mpc_target', mpc_target_trajectories, queue_size=10) + self.tf_listener = tf.TransformListener() + self.rate = rospy.Rate(0.5) # 1 Hz + + # 初始化目标位置列表 + self.target_positions = [ + # [0.307, -0.261, 0.608, 0.55, 0.342, -0.402, 0.647], + # [-0.297, 0.043, -0.033, 0.697, -0.265, -0.585, 0.316], + # [0.247, 0.41, 0.023, 0.0, 0.263, -0.0, 0.965], + # [0.496, -0.185, 0.127, 0.0, 0.263, -0.0, 0.965], + [0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], + [0.495, 0.61, 0.667, 0.0, 0.0, 0.0, 1.0], + [0.675, 0.157, 0.339, 0.0, 0.0, 0.0, 1.0] + + ] + + self.current_target_index = 0 + + def create_mpc_state(self, data): + state = mpc_state() + state.value = data + return state + + def create_mpc_input(self, data): + input = mpc_input() + input.value = data + return input + + def publish_target(self, target_position): + # create and publish message + input_data = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + msg = mpc_target_trajectories() + current_time = rospy.Time.now().to_sec() + msg.timeTrajectory = [current_time] + msg.stateTrajectory = [self.create_mpc_state(target_position)] + msg.inputTrajectory = [self.create_mpc_state(input_data)] + rospy.loginfo("Publishing target trajectories: " + str(msg)) + self.publisher.publish(msg) + + def get_hand_position(self): + try: + # wait tf transform + self.tf_listener.waitForTransform('/world', '/panda_leftfinger', rospy.Time(), rospy.Duration(6)) + (trans, rot) = self.tf_listener.lookupTransform('/world', '/panda_leftfinger', rospy.Time(0)) + return trans + except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: + rospy.logerr(f"TF error: {e}") + return None + + def check_position(self, target_position, current_position, tolerance=0.05): + # To check position if arrived within tolerance + for t, c in zip(target_position, current_position): + if abs(t - c) > tolerance: + return False + return True + + + def run(self): + iteration = 0 + start_time = time.time() + while not rospy.is_shutdown(): + if self.current_target_index < len(self.target_positions): + # Get current position + target_position = self.target_positions[self.current_target_index] + + # publish target position + self.publish_target(target_position) + hand_position = self.get_hand_position() + if hand_position: + rospy.loginfo(f"Current franka hand position: {hand_position}") + + # check position if in tolerance + if self.check_position(target_position, hand_position): + rospy.loginfo("Position arrived within tolerance, move to next one") + # next position + self.current_target_index += 1 + start_time = time.time() + else: + rospy.logwarn("Position does not match within tolerance.") + # To check if spend too much time on this movement + if time.time() - start_time > 600: + rospy.logerr("Timeout: Position check exceeded 600 seconds, please check if there is any error") + rospy.signal_shutdown("Timeout") + + else: + rospy.loginfo("complete iteration " + str(iteration)) + iteration += 1 + self.current_target_index = 0 + if iteration > max_iteration: + rospy.signal_shutdown("Target iteration reach") + + self.rate.sleep() + +if __name__ == '__main__': + node = MobileManipulatorNode() + node.run() + diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/scripts/ridgeback_control.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/scripts/ridgeback_control.py new file mode 100644 index 000000000..82e746ca6 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/scripts/ridgeback_control.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python + +import rospy +import tf +import time +from ocs2_msgs.msg import mpc_target_trajectories, mpc_state, mpc_input +from geometry_msgs.msg import Pose +from tf2_msgs.msg import TFMessage + +max_iteration = 10 + +class MobileManipulatorNode: + def __init__(self): + rospy.init_node('mobile_manipulator_ridgeback_control_node', anonymous=True) + self.publisher = rospy.Publisher('/mobile_manipulator_mpc_target', mpc_target_trajectories, queue_size=10) + self.tf_listener = tf.TransformListener() + self.rate = rospy.Rate(1) # 1 Hz + + # 初始化目标位置列表 + self.target_positions = [ + [0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], + [0.721, 0.754, 0.572, 0.0, -0.114, 0.0, 0.993], + [0.323, 0.594, 0.982, 0.113, -0.004, -0.992, 0.04] + ] + + self.current_target_index = 0 + + def create_mpc_state(self, data): + state = mpc_state() + state.value = data + return state + + def create_mpc_input(self, data): + input = mpc_input() + input.value = data + return input + + def publish_target(self, target_position): + # create and publish message + input_data = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + msg = mpc_target_trajectories() + current_time = rospy.Time.now().to_sec() + msg.timeTrajectory = [current_time] + msg.stateTrajectory = [self.create_mpc_state(target_position)] + msg.inputTrajectory = [self.create_mpc_state(input_data)] + rospy.loginfo("Publishing target trajectories: " + str(msg)) + self.publisher.publish(msg) + + def get_hand_position(self): + try: + # wait tf transform + self.tf_listener.waitForTransform('/world', '/ur_arm_flange', rospy.Time(), rospy.Duration(6)) + (trans, rot) = self.tf_listener.lookupTransform('/world', '/ur_arm_flange', rospy.Time(0)) + return trans + except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: + rospy.logerr(f"TF error: {e}") + return None + + def check_position(self, target_position, current_position, tolerance=0.02): + # To check position if arrived within tolerance + for t, c in zip(target_position, current_position): + # print("t is {}, c is {}".format(t, c)) + if abs(t - c) > tolerance: + return False + return True + + + def run(self): + iteration = 0 + start_time = time.time() + while not rospy.is_shutdown(): + if self.current_target_index < len(self.target_positions): + # Get current position + target_position = self.target_positions[self.current_target_index] + + # publish target position + self.publish_target(target_position) + hand_position = self.get_hand_position() + if hand_position: + rospy.loginfo(f"Current ridgeback hand position: {hand_position}") + + # check position if in tolerance + if self.check_position(target_position, hand_position): + rospy.loginfo("Position arrived within tolerance, move to next one") + # next position + self.current_target_index += 1 + start_time = time.time() + else: + rospy.logwarn("Position does not match within tolerance.") + # To check if spend too much time on this movement + if time.time() - start_time > 600: + rospy.logerr("Timeout: Position check exceeded 600 seconds, please check if there is any error") + rospy.signal_shutdown("Timeout") + + else: + rospy.loginfo("complete iteration " + str(iteration)) + iteration += 1 + self.current_target_index = 0 + if iteration > max_iteration: + rospy.signal_shutdown("Target iteration reach") + + self.rate.sleep() + +if __name__ == '__main__': + node = MobileManipulatorNode() + node.run() + From e5f9e55c9308e18d67422a18dbb8b7651cb9f50b Mon Sep 17 00:00:00 2001 From: jzhao6x <88374403+jzhao6x@users.noreply.github.com> Date: Wed, 14 May 2025 10:24:41 +0800 Subject: [PATCH 03/42] update for benchmarking (#3) * modify description name * add scripts to control robot * update for benchmarking --- ocs2_core/include/ocs2_core/misc/Benchmark.h | 20 ++++- .../ocs2_legged_robot/config/mpc/task.info | 6 +- .../scripts/cpu_monitor_core.py | 40 +++++++++ .../scripts/cpu_monitor_usage.py | 89 +++++++++++++++++++ .../config/franka/task.info | 2 +- .../config/ridgeback_ur5/task.info | 2 +- .../mrt/MRT_ROS_Dummy_Loop.h | 2 + .../src/mpc/MPC_ROS_Interface.cpp | 2 +- .../src/mrt/MRT_ROS_Dummy_Loop.cpp | 14 +++ 9 files changed, 167 insertions(+), 10 deletions(-) create mode 100755 ocs2_robotic_examples/ocs2_legged_robot_ros/scripts/cpu_monitor_core.py create mode 100755 ocs2_robotic_examples/ocs2_legged_robot_ros/scripts/cpu_monitor_usage.py diff --git a/ocs2_core/include/ocs2_core/misc/Benchmark.h b/ocs2_core/include/ocs2_core/misc/Benchmark.h index a551e7fca..bc56d43a8 100644 --- a/ocs2_core/include/ocs2_core/misc/Benchmark.h +++ b/ocs2_core/include/ocs2_core/misc/Benchmark.h @@ -30,7 +30,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once #include - +#include #include "ocs2_core/Types.h" namespace ocs2 { @@ -46,7 +46,9 @@ class RepeatedTimer { totalTime_(std::chrono::nanoseconds::zero()), maxIntervalTime_(std::chrono::nanoseconds::zero()), lastIntervalTime_(std::chrono::nanoseconds::zero()), - startTime_(std::chrono::steady_clock::now()) {} + startTime_(std::chrono::steady_clock::now()) { + maxIntervalIndex_ = -1; + } /** * Reset the timer statistics @@ -56,6 +58,7 @@ class RepeatedTimer { totalTime_ = std::chrono::nanoseconds::zero(); maxIntervalTime_ = std::chrono::nanoseconds::zero(); lastIntervalTime_ = std::chrono::nanoseconds::zero(); + maxIntervalIndex_ = -1; } /** @@ -69,10 +72,13 @@ class RepeatedTimer { void endTimer() { auto endTime = std::chrono::steady_clock::now(); lastIntervalTime_ = std::chrono::duration_cast(endTime - startTime_); - maxIntervalTime_ = std::max(maxIntervalTime_, lastIntervalTime_); + if (lastIntervalTime_ > maxIntervalTime_) { + maxIntervalTime_ = lastIntervalTime_; + maxIntervalIndex_ = numTimedIntervals_; + } totalTime_ += lastIntervalTime_; numTimedIntervals_++; - }; + } /** * @return Number of intervals that were timed @@ -99,12 +105,18 @@ class RepeatedTimer { */ scalar_t getAverageInMilliseconds() const { return getTotalInMilliseconds() / numTimedIntervals_; } + /** + * @return The index of the interval with the maximum duration + */ + int getMaxIntervalIndex() const { return maxIntervalIndex_; } + private: int numTimedIntervals_; std::chrono::nanoseconds totalTime_; std::chrono::nanoseconds maxIntervalTime_; std::chrono::nanoseconds lastIntervalTime_; std::chrono::steady_clock::time_point startTime_; + int maxIntervalIndex_; }; } // namespace benchmark diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info index 017dc40c3..13fd0cb56 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info @@ -1,4 +1,4 @@ -centroidalModelType 1 // 0: FullCentroidalDynamics, 1: Single Rigid Body Dynamics +centroidalModelType 0 // 0: FullCentroidalDynamics, 1: Single Rigid Body Dynamics legged_robot_interface { @@ -13,7 +13,7 @@ model_settings phaseTransitionStanceTime 0.4 verboseCppAd true - recompileLibrariesCppAd true + recompileLibrariesCppAd false modelFolderCppAd /tmp/ocs2 } @@ -136,7 +136,7 @@ mpc solutionTimeWindow -1 ; maximum [s] coldStart false - debugPrint false + debugPrint true mpcDesiredFrequency 50 ; [Hz] mrtDesiredFrequency 400 ; [Hz] diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/scripts/cpu_monitor_core.py b/ocs2_robotic_examples/ocs2_legged_robot_ros/scripts/cpu_monitor_core.py new file mode 100755 index 000000000..980097ebd --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/scripts/cpu_monitor_core.py @@ -0,0 +1,40 @@ +import psutil +import time +import signal +import sys + +output_file = f"cpu_frequencies_{time.strftime('%Y-%m-%d-%H-%M-%S')}.log" +max_freqs = [] + +def record_cpu_frequencies(): + global max_freqs + with open(output_file, "a") as f: + while True: + timestamp = time.strftime("%Y-%m-%d %H:%M:%S") + # Get frequency for each CPU core + cpu_freqs = psutil.cpu_freq(percpu=True) + # init frequency list + if not max_freqs: + max_freqs = [0.0] * len(cpu_freqs) + # record frequency and update max frequency + freq_data = [f"{timestamp}"] + for i, freq in enumerate(cpu_freqs): + current_freq = freq.current + freq_data.append(f"Core {i}: {current_freq} MHz") + if current_freq > max_freqs[i]: + max_freqs[i] = current_freq + f.write(", ".join(freq_data) + "\n") + f.flush() + time.sleep(1) + +def signal_handler(sig, frame): + print("\nCtrl+C detected. Printing highest frequencies recorded for each core:") + for i, max_freq in enumerate(max_freqs): + print(f"Core {i}: {round(max_freq)} MHz") + sys.exit(0) + +if __name__ == "__main__": + signal.signal(signal.SIGINT, signal_handler) + print(f"Recording CPU frequencies to {output_file}. Press Ctrl+C to stop.") + record_cpu_frequencies() + diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/scripts/cpu_monitor_usage.py b/ocs2_robotic_examples/ocs2_legged_robot_ros/scripts/cpu_monitor_usage.py new file mode 100755 index 000000000..9506744b3 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/scripts/cpu_monitor_usage.py @@ -0,0 +1,89 @@ +import psutil +import time +import signal +import sys + +output_file = f"cpu_usage_{time.strftime('%Y-%m-%d-%H-%M-%S')}.log" + +cpu_usages = [] +max_cpu_usage = 0.0 + +# process keyword options. Find the process id by these keywords +# 1~3 for legged_robot, 4~5 for franka and ridgeback +process_options = { + 1: "legged_robot_ddp_mpc", + 2: "legged_robot_sqp_mpc", + 3: "legged_robot_dummy", + 4: "mpc_node", + 5: "dummy_mrt_node" +} + +def find_process_by_keyword(keyword): + """Find the process by keywords""" + for proc in psutil.process_iter(['pid', 'name', 'cmdline']): + try: + if keyword in proc.info['name'] or keyword in ' '.join(proc.info['cmdline']): + return proc + except (psutil.NoSuchProcess, psutil.AccessDenied, psutil.ZombieProcess): + pass + return None + +def record_cpu_usage(proc): + global max_cpu_usage + with open(output_file, "a") as f: + while True: + try: + timestamp = time.strftime("%Y-%m-%d %H:%M:%S") + # Get cpu usage for each process + cpu_usage = proc.cpu_percent(interval=1) + # record cpu usage and update max cup usage + cpu_usages.append(cpu_usage) + if cpu_usage > max_cpu_usage: + max_cpu_usage = cpu_usage + # write result to a file + f.write(f"{timestamp}, CPU Usage: {cpu_usage}%\n") + f.flush() + except psutil.NoSuchProcess: + print("\nProcess no longer exists.") + calculate_and_print_cpu_usage() + sys.exit(0) + +def calculate_and_print_cpu_usage(): + if cpu_usages: + avg_cpu_usage = round(sum(cpu_usages) / len(cpu_usages), 1) + print(f"Average CPU Usage: {avg_cpu_usage}%") + print(f"Maximum CPU Usage: {max_cpu_usage}%") + else: + print("No CPU usage data recorded.") + +def signal_handler(sig, frame): + print("\nCtrl+C detected.") + calculate_and_print_cpu_usage() + sys.exit(0) + +if __name__ == "__main__": + # if encounter Ctrl+C then save the results + signal.signal(signal.SIGINT, signal_handler) + + print("Select the process to monitor:") + for key, value in process_options.items(): + print(f"{key} - {value}") + + # Get input + try: + option = int(input("Enter the number corresponding to the process: ")) + if option not in process_options: + raise ValueError("Invalid option") + except ValueError as e: + print(f"Error: {e}") + sys.exit(1) + + keyword = process_options[option] + + process = find_process_by_keyword(keyword) + if process: + print(f"Found process: {process.info['name']} (PID: {process.info['pid']})") + print(f"Recording CPU usage to {output_file}. Press Ctrl+C to stop.") + record_cpu_usage(process) + else: + print(f"No process found with keyword: {keyword}") diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/config/franka/task.info b/ocs2_robotic_examples/ocs2_mobile_manipulator/config/franka/task.info index c0c01f92d..2f775ca15 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/config/franka/task.info +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/config/franka/task.info @@ -79,7 +79,7 @@ mpc solutionTimeWindow 0.2 ; [s] coldStart false - debugPrint false + debugPrint true mpcDesiredFrequency 100 ; [Hz] mrtDesiredFrequency 400 ; [Hz] diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/config/ridgeback_ur5/task.info b/ocs2_robotic_examples/ocs2_mobile_manipulator/config/ridgeback_ur5/task.info index 0c97d2a6a..c708091c5 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/config/ridgeback_ur5/task.info +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/config/ridgeback_ur5/task.info @@ -77,7 +77,7 @@ mpc solutionTimeWindow 0.2 ; [s] coldStart false - debugPrint false + debugPrint true mpcDesiredFrequency 100 ; [Hz] mrtDesiredFrequency 400 ; [Hz] diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h index d1c222f21..1d27c4659 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h @@ -99,6 +99,8 @@ class MRT_ROS_Dummy_Loop { scalar_t mrtDesiredFrequency_; scalar_t mpcDesiredFrequency_; + + benchmark::RepeatedTimer mrtTimer_; }; } // namespace ocs2 diff --git a/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp b/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp index 2872fd8d8..4178bb1bb 100644 --- a/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp +++ b/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp @@ -271,7 +271,7 @@ void MPC_ROS_Interface::mpcObservationCallback(const ocs2_msgs::mpc_observation: if (mpc_.settings().debugPrint_) { std::cerr << '\n'; std::cerr << "\n### MPC_ROS Benchmarking"; - std::cerr << "\n### Maximum : " << mpcTimer_.getMaxIntervalInMilliseconds() << "[ms]."; + std::cerr << "\n### Maximum : " << mpcTimer_.getMaxIntervalInMilliseconds() << "[ms] in interval " << mpcTimer_.getMaxIntervalIndex() << "."; std::cerr << "\n### Average : " << mpcTimer_.getAverageInMilliseconds() << "[ms]."; std::cerr << "\n### Latest : " << mpcTimer_.getLastIntervalInMilliseconds() << "[ms]." << std::endl; } diff --git a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp index d41995ae5..bcbc29865 100644 --- a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp +++ b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp @@ -27,6 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ +#include #include "ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h" namespace ocs2 { @@ -104,9 +105,15 @@ void MRT_ROS_Dummy_Loop::synchronizedDummyLoop(const SystemObservation& initObse std::cout << "<<< New MPC policy starting at " << mrt_.getPolicy().timeTrajectory_.front() << "\n"; } + // measure the delay in running MRT + mrtTimer_.startTimer(); + // Forward simulation currentObservation = forwardSimulation(currentObservation); + // measure the delay for sending ROS messages + mrtTimer_.endTimer(); + // User-defined modifications before publishing modifyObservation(currentObservation); @@ -121,6 +128,13 @@ void MRT_ROS_Dummy_Loop::synchronizedDummyLoop(const SystemObservation& initObse observer->update(currentObservation, mrt_.getPolicy(), mrt_.getCommand()); } + //display + std::cerr << '\n'; + std::cerr << "\n### FOR MRT_ROS Benchmarking"; + std::cerr << "\n### Maximum : " << mrtTimer_.getMaxIntervalInMilliseconds() << "[ms] in interval " << mrtTimer_.getMaxIntervalIndex() << "."; + std::cerr << "\n### Average : " << mrtTimer_.getAverageInMilliseconds() << "[ms]."; + std::cerr << "\n### Latest : " << mrtTimer_.getLastIntervalInMilliseconds() << "[ms]." << std::endl; + ++loopCounter; ros::spinOnce(); simRate.sleep(); From 121b215ea6badbf0d10efaeb203ad360ced487cd Mon Sep 17 00:00:00 2001 From: "Yu, Yan" Date: Tue, 20 May 2025 11:29:43 +0800 Subject: [PATCH 04/42] Fix compile error (#4) --- .../include/ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h | 1 + 1 file changed, 1 insertion(+) diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h index 1d27c4659..1b7f56b45 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h @@ -31,6 +31,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ros_interfaces/mrt/DummyObserver.h" #include "ocs2_ros_interfaces/mrt/MRT_ROS_Interface.h" +#include "ocs2_core/misc/Benchmark.h" namespace ocs2 { From ed57c39eae777b6465e79ed1124d5befb31d3248 Mon Sep 17 00:00:00 2001 From: "Yu, Yan" Date: Wed, 21 May 2025 18:15:56 +0800 Subject: [PATCH 05/42] Disable non necessary build (#5) * Disable non necessary build * ::Remove gnome debug from mobile manipulator launch file --------- Co-authored-by: Zhao, JieX --- .github/workflows/ros-build-test.yml | 57 ++++++++++--------- ocs2_doc/CATKIN_IGNORE | 0 ocs2_mpcnet/CATKIN_IGNORE | 0 ocs2_raisim/CATKIN_IGNORE | 0 .../launch/include/mobile_manipulator.launch | 4 +- 5 files changed, 32 insertions(+), 29 deletions(-) create mode 100644 ocs2_doc/CATKIN_IGNORE create mode 100644 ocs2_mpcnet/CATKIN_IGNORE create mode 100644 ocs2_raisim/CATKIN_IGNORE diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index 0f3ac9433..8d6072efb 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -16,7 +16,7 @@ jobs: build_type: [ Debug, Release ] # environment: regular Ubuntu with a vanilla ROS container - runs-on: ubuntu-latest + runs-on: [sandbox.prod.amr.dind] container: image: ros:noetic @@ -30,49 +30,52 @@ jobs: - name: System deps run: | apt-get update + DEBIAN_FRONTEND=noninteractive apt-get install keyboard-configuration --assume-yes apt-get install -y git ninja-build liburdfdom-dev liboctomap-dev libassimp-dev checkinstall wget rsync - uses: actions/checkout@v2 with: path: src/ocs2 - - name: Rosdep - run: | - rosdep update - rosdep install --from-paths src --ignore-src -r -y - - name: Checkout dependencies run: | git clone --recurse-submodules https://github.com/leggedrobotics/pinocchio.git src/pinocchio git clone --recurse-submodules https://github.com/leggedrobotics/hpp-fcl.git src/hpp-fcl git clone https://github.com/leggedrobotics/ocs2_robotic_assets.git src/ocs2_robotic_assets + git clone https://github.com/leggedrobotics/elevation_mapping_cupy.git src/elevation_mapping_cupy + git clone https://github.com/ANYbotics/grid_map.git src/grid_map - - name: Install RaiSim + - name: Rosdep run: | - git clone --depth 1 https://github.com/raisimTech/raisimLib.git -b v1.1.01 src/raisim_tech - cd src/raisim_tech - mkdir build - cd build - cmake .. -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)") - make -j4 && checkinstall + rosdep update + rosdep install --from-paths src --ignore-src -r -y + + # - name: Install RaiSim + # run: | + # git clone --depth 1 https://github.com/raisimTech/raisimLib.git -b v1.1.01 src/raisim_tech + # cd src/raisim_tech + # mkdir build + # cd build + # cmake .. -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)") + # make -j4 && checkinstall - - name: Install ONNX Runtime - run: | - wget https://github.com/microsoft/onnxruntime/releases/download/v1.7.0/onnxruntime-linux-x64-1.7.0.tgz -P tmp/microsoft - tar xf tmp/microsoft/onnxruntime-linux-x64-1.7.0.tgz -C tmp/microsoft - rsync -a tmp/microsoft/onnxruntime-linux-x64-1.7.0/include/ /usr/local/include/onnxruntime - rsync -a tmp/microsoft/onnxruntime-linux-x64-1.7.0/lib/ /usr/local/lib - rsync -a src/ocs2/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/ /usr/local/share/onnxruntime + # - name: Install ONNX Runtime + # run: | + # wget https://github.com/microsoft/onnxruntime/releases/download/v1.7.0/onnxruntime-linux-x64-1.7.0.tgz -P tmp/microsoft + # tar xf tmp/microsoft/onnxruntime-linux-x64-1.7.0.tgz -C tmp/microsoft + # rsync -a tmp/microsoft/onnxruntime-linux-x64-1.7.0/include/ /usr/local/include/onnxruntime + # rsync -a tmp/microsoft/onnxruntime-linux-x64-1.7.0/lib/ /usr/local/lib + # rsync -a src/ocs2/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/ /usr/local/share/onnxruntime - name: Build (${{ matrix.build_type }}) shell: bash run: | source /opt/ros/noetic/setup.bash - catkin_make_isolated --use-ninja --merge --only-pkg-with-deps ocs2 --cmake-args -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} + catkin build -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} - - name: Test - shell: bash - run: | - source devel_isolated/setup.bash - catkin_make_isolated --use-ninja --merge --only-pkg-with-deps ocs2 --catkin-make-args run_tests - catkin_test_results + # - name: Test + # shell: bash + # run: | + # source devel_isolated/setup.bash + # catkin_make_isolated --use-ninja --merge --only-pkg-with-deps ocs2 --catkin-make-args run_tests + # catkin_test_results diff --git a/ocs2_doc/CATKIN_IGNORE b/ocs2_doc/CATKIN_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_mpcnet/CATKIN_IGNORE b/ocs2_mpcnet/CATKIN_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_raisim/CATKIN_IGNORE b/ocs2_raisim/CATKIN_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator.launch b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator.launch index 9f02a6f72..b3f30bfa1 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator.launch +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/include/mobile_manipulator.launch @@ -25,12 +25,12 @@ + output="screen" launch-prefix="" /> + output="screen" launch-prefix="" /> From 8030a2739713c6299a0c75d8c4de4c17a27fa29f Mon Sep 17 00:00:00 2001 From: "Yu, Yan" Date: Mon, 26 May 2025 17:15:07 +0800 Subject: [PATCH 06/42] :Fix CI build (#6) --- .github/workflows/ros-build-test.yml | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index 8d6072efb..adcbd7e82 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -13,10 +13,10 @@ jobs: strategy: fail-fast: false matrix: - build_type: [ Debug, Release ] + build_type: [Release] # environment: regular Ubuntu with a vanilla ROS container - runs-on: [sandbox.prod.amr.dind] + runs-on: [self-hosted, Linux, X64] container: image: ros:noetic @@ -26,11 +26,20 @@ jobs: pwd uname -r lsb_release -a + ls /etc/apt/sources.list.d/ - name: System deps + shell: bash run: | apt-get update + apt-get upgrade -y + source /opt/ros/noetic/setup.bash DEBIAN_FRONTEND=noninteractive apt-get install keyboard-configuration --assume-yes + apt-get install libglpk-dev -y + apt-get install libmpfr-dev -y + apt-get install python3-catkin-tools -y + apt-get install ros-noetic-cv-bridge -y + apt-get install liburdfdom-dev liboctomap-dev libassimp-dev libeigen3-dev -y apt-get install -y git ninja-build liburdfdom-dev liboctomap-dev libassimp-dev checkinstall wget rsync - uses: actions/checkout@v2 From 9a8f7e74a825b4f464488a6bee7b317ca956e9c8 Mon Sep 17 00:00:00 2001 From: "Yu, Yan" Date: Tue, 27 May 2025 14:46:16 +0800 Subject: [PATCH 07/42] Remove redundant steps for CI build (#8) --- .github/workflows/ros-build-test.yml | 40 +++++++++++++++++++--------- 1 file changed, 28 insertions(+), 12 deletions(-) diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index adcbd7e82..c26a98189 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -10,15 +10,14 @@ jobs: build: # build both Debug and Release mode - strategy: - fail-fast: false - matrix: - build_type: [Release] + #strategy: + # fail-fast: false + # matrix: + # build_type: [Release] # environment: regular Ubuntu with a vanilla ROS container runs-on: [self-hosted, Linux, X64] - container: - image: ros:noetic + container: ros:noetic steps: - name: Environment Info @@ -26,21 +25,18 @@ jobs: pwd uname -r lsb_release -a - ls /etc/apt/sources.list.d/ - name: System deps shell: bash run: | apt-get update - apt-get upgrade -y source /opt/ros/noetic/setup.bash DEBIAN_FRONTEND=noninteractive apt-get install keyboard-configuration --assume-yes apt-get install libglpk-dev -y apt-get install libmpfr-dev -y apt-get install python3-catkin-tools -y apt-get install ros-noetic-cv-bridge -y - apt-get install liburdfdom-dev liboctomap-dev libassimp-dev libeigen3-dev -y - apt-get install -y git ninja-build liburdfdom-dev liboctomap-dev libassimp-dev checkinstall wget rsync + apt-get install git liburdfdom-dev liboctomap-dev libassimp-dev libeigen3-dev -y - uses: actions/checkout@v2 with: @@ -48,10 +44,30 @@ jobs: - name: Checkout dependencies run: | + if [ -d "src/pinocchio" ]; then + echo "Cleaning up existing src/pinocchio directory..." + rm -rf src/pinocchio + fi git clone --recurse-submodules https://github.com/leggedrobotics/pinocchio.git src/pinocchio + if [ -d "src/hpp-fcl" ]; then + echo "Cleaning up existing src/hpp-fcl directory..." + rm -rf src/hpp-fcl + fi git clone --recurse-submodules https://github.com/leggedrobotics/hpp-fcl.git src/hpp-fcl + if [ -d "src/ocs2_robotic_assets" ]; then + echo "Cleaning up existing src/ocs2_robotic_assets directory..." + rm -rf src/ocs2_robotic_assets + fi git clone https://github.com/leggedrobotics/ocs2_robotic_assets.git src/ocs2_robotic_assets + if [ -d "src/elevation_mapping_cupy" ]; then + echo "Cleaning up existing src/evelation_cupy directory..." + rm -rf src/elevation_mapping_cupy + fi git clone https://github.com/leggedrobotics/elevation_mapping_cupy.git src/elevation_mapping_cupy + if [ -d "src/grid_map" ]; then + echo "Cleaning up existing src/grid_map directory..." + rm -rf src/grid_map + fi git clone https://github.com/ANYbotics/grid_map.git src/grid_map - name: Rosdep @@ -76,11 +92,11 @@ jobs: # rsync -a tmp/microsoft/onnxruntime-linux-x64-1.7.0/lib/ /usr/local/lib # rsync -a src/ocs2/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/ /usr/local/share/onnxruntime - - name: Build (${{ matrix.build_type }}) + - name: Build shell: bash run: | source /opt/ros/noetic/setup.bash - catkin build -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} + catkin build -DCMAKE_BUILD_TYPE=Release # - name: Test # shell: bash From ce066e133bb80ac1b52aec240b126fcb933c2294 Mon Sep 17 00:00:00 2001 From: jzhao6x <88374403+jzhao6x@users.noreply.github.com> Date: Wed, 28 May 2025 10:25:13 +0800 Subject: [PATCH 08/42] update for ocs2 build only test (#7) --- .../scripts/cpu_monitor_core.py | 17 ++++--- .../scripts/legged_robot_control.py | 44 ++++++++++++------- .../scripts/franka_control.py | 4 +- .../scripts/ridgeback_control.py | 4 +- 4 files changed, 44 insertions(+), 25 deletions(-) mode change 100644 => 100755 ocs2_robotic_examples/ocs2_legged_robot_ros/scripts/legged_robot_control.py diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/scripts/cpu_monitor_core.py b/ocs2_robotic_examples/ocs2_legged_robot_ros/scripts/cpu_monitor_core.py index 980097ebd..2ad1b08fc 100755 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/scripts/cpu_monitor_core.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/scripts/cpu_monitor_core.py @@ -5,32 +5,39 @@ output_file = f"cpu_frequencies_{time.strftime('%Y-%m-%d-%H-%M-%S')}.log" max_freqs = [] +total_freqs = [] +sample_counts = [] def record_cpu_frequencies(): - global max_freqs + global max_freqs, total_freqs, sample_counts with open(output_file, "a") as f: while True: timestamp = time.strftime("%Y-%m-%d %H:%M:%S") # Get frequency for each CPU core cpu_freqs = psutil.cpu_freq(percpu=True) - # init frequency list + # init frequency lists if not max_freqs: max_freqs = [0.0] * len(cpu_freqs) - # record frequency and update max frequency + total_freqs = [0.0] * len(cpu_freqs) + sample_counts = [0] * len(cpu_freqs) + # record frequency and update max frequency and total frequency freq_data = [f"{timestamp}"] for i, freq in enumerate(cpu_freqs): current_freq = freq.current freq_data.append(f"Core {i}: {current_freq} MHz") if current_freq > max_freqs[i]: max_freqs[i] = current_freq + total_freqs[i] += current_freq + sample_counts[i] += 1 f.write(", ".join(freq_data) + "\n") f.flush() time.sleep(1) def signal_handler(sig, frame): - print("\nCtrl+C detected. Printing highest frequencies recorded for each core:") + print("\nCtrl+C detected. Printing highest and average frequencies recorded for each core:") for i, max_freq in enumerate(max_freqs): - print(f"Core {i}: {round(max_freq)} MHz") + avg_freq = total_freqs[i] / sample_counts[i] if sample_counts[i] > 0 else 0 + print(f"Core {i}: max {round(max_freq)} MHz, avg {round(avg_freq)} MHz") sys.exit(0) if __name__ == "__main__": diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/scripts/legged_robot_control.py b/ocs2_robotic_examples/ocs2_legged_robot_ros/scripts/legged_robot_control.py old mode 100644 new mode 100755 index a9b21a69d..9e1b7918f --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/scripts/legged_robot_control.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/scripts/legged_robot_control.py @@ -11,8 +11,8 @@ target_distance_2 = (-1.0, -1.0, 0.0, 0.0) initial_translation = (0, 0, 0) translation = None -tolerance = 0.01 -max_iteration = 10 +tolerance = 0.03 +max_iteration = 300 def set_gait(): @@ -47,15 +47,16 @@ def run_legged_robot_target(target_distance): # Input XYZ and Yaw displacements input_target = ' '.join(map(str, target_distance)) - # print(input_target) + print("iteration input: " + input_target) process.stdin.write(input_target + '\n') process.stdin.flush() # To wait command published + time.sleep(1) while True: output = process.stdout.readline() if output: - # print(output.strip() + '\n') + print("output is: " + output.strip()) if "The following command is published" in output: rospy.loginfo("Detected the target command published. Input: " + input_target) break @@ -84,14 +85,15 @@ def stop(self): self.subscriber = None rospy.signal_shutdown('Listener stopped') - def calculate_distance(self, trans1, trans2): - """Calculate two translations distance""" - dx = trans2[0] - trans1[0] - dy = trans2[1] - trans1[1] - return math.sqrt(dx**2 + dy**2) + # def calculate_distance(self, trans1, trans2): + # """Calculate two translations distance""" + # dx = trans2[0] - trans1[0] + # dy = trans2[1] - trans1[1] + # return math.sqrt(dx**2 + dy**2) def callback(self, data, target_distance): global translation, initial_translation + rate = rospy.Rate(100) for transform in data.transforms: if transform.child_frame_id == "base": translation = ( @@ -100,18 +102,21 @@ def callback(self, data, target_distance): transform.transform.translation.z ) - # calculate two translations distance - distance = self.calculate_distance(initial_translation, translation) - target_distance_xy = math.sqrt(target_distance[0]**2 + target_distance[1]**2) - - if abs(distance - target_distance_xy) <= tolerance: + # # calculate two translations distance + # distance = self.calculate_distance(initial_translation, translation) + # target_distance_xy = math.sqrt(target_distance[0]**2 + target_distance[1]**2) + dx = round(abs(translation[0] - initial_translation[0]), 2) + dy = round(abs(translation[1] - initial_translation[1]), 2) + + if 1-dx <= tolerance and 1-dy <= tolerance: rospy.loginfo("Target reached within tolerance. Translation: x=%f, y=%f, z=%f", translation[0], translation[1], translation[2]) + # time.sleep(0.5) initial_translation = translation # update initial translation for next movement - rospy.loginfo('movement ' + str(self.iteration)) + rospy.loginfo('Start movement ' + str(self.iteration)) if self.iteration % 2 == 0: target_distance = target_distance_1 else: @@ -120,6 +125,13 @@ def callback(self, data, target_distance): self.iteration += 1 if self.iteration > max_iteration: rospy.signal_shutdown("Target reached") + # else: + # rospy.loginfo("Current Translation: x=%f, y=%f, z=%f", + # translation[0], + # translation[1], + # translation[2]) + # rospy.loginfo("x distance=%f, y distance_xy=%f", dx, dy) + # rate.sleep() def main(): @@ -133,4 +145,4 @@ def main(): if __name__ == "__main__": main() - + \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/scripts/franka_control.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/scripts/franka_control.py index 3fa3f028d..e720dec60 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/scripts/franka_control.py +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/scripts/franka_control.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import rospy import tf @@ -7,7 +7,7 @@ from geometry_msgs.msg import Pose from tf2_msgs.msg import TFMessage -max_iteration = 10 +max_iteration = 300 class MobileManipulatorNode: def __init__(self): diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/scripts/ridgeback_control.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/scripts/ridgeback_control.py index 82e746ca6..c5e7fb017 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/scripts/ridgeback_control.py +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/scripts/ridgeback_control.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import rospy import tf @@ -7,7 +7,7 @@ from geometry_msgs.msg import Pose from tf2_msgs.msg import TFMessage -max_iteration = 10 +max_iteration = 300 class MobileManipulatorNode: def __init__(self): From 2f174ae5b1b90b12ef57516f7ec48f86b90940db Mon Sep 17 00:00:00 2001 From: "Yu, Yan" Date: Fri, 15 Aug 2025 14:50:16 +0800 Subject: [PATCH 09/42] Add default reviewers (#9) * Add default reviewers --- .github/CODEOWNERS | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 .github/CODEOWNERS diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS new file mode 100644 index 000000000..267fd7a5f --- /dev/null +++ b/.github/CODEOWNERS @@ -0,0 +1,4 @@ +# OCS2 Intel-maintained repo default reviewers + +# Default Reviewers: +* @RoboticsYY @baihe-liu @AnnikaWU \ No newline at end of file From 5a1d92c3eb7edc20ac905e2f82f63ae96f9a8dbd Mon Sep 17 00:00:00 2001 From: "Yu, Yan" Date: Fri, 15 Aug 2025 16:23:05 +0800 Subject: [PATCH 10/42] Trigger CI on ros2 branch (#10) --- .github/workflows/ros-build-test.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index c26a98189..50b9cc720 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -2,9 +2,9 @@ name: Build and Test OCS2 on: push: - branches: [ main ] + branches: [ ros2 ] pull_request: - branches: [ main ] + branches: [ ros2 ] jobs: build: @@ -72,7 +72,7 @@ jobs: - name: Rosdep run: | - rosdep update + rosdep update --rosdistro noetic rosdep install --from-paths src --ignore-src -r -y # - name: Install RaiSim From 3de67151330abcb301c9283bfd168487e2ceb8bd Mon Sep 17 00:00:00 2001 From: jzhao6x <88374403+jzhao6x@users.noreply.github.com> Date: Mon, 18 Aug 2025 15:33:45 +0800 Subject: [PATCH 11/42] CI: update build flow for ros2 (#11) * update build flow for ros2 * Add default reviewers (#9) * Add default reviewers * Trigger CI on ros2 branch (#10) * rosdep update with rosdistro * clear dir before build * add check to remove some directories --------- Co-authored-by: Yu, Yan --- .github/workflows/ros-build-test.yml | 21 +++++++++------------ ocs2/COLCON_IGNORE | 0 ocs2_core/COLCON_IGNORE | 0 ocs2_ddp/COLCON_IGNORE | 0 ocs2_doc/COLCON_IGNORE | 0 ocs2_frank_wolfe/COLCON_IGNORE | 0 ocs2_ipm/COLCON_IGNORE | 0 ocs2_mpc/COLCON_IGNORE | 0 ocs2_mpcnet/COLCON_IGNORE | 0 ocs2_msgs/COLCON_IGNORE | 0 ocs2_oc/COLCON_IGNORE | 0 ocs2_ocs2/COLCON_IGNORE | 0 ocs2_perceptive/COLCON_IGNORE | 0 ocs2_pinocchio/COLCON_IGNORE | 0 ocs2_python_interface/COLCON_IGNORE | 0 ocs2_raisim/COLCON_IGNORE | 0 ocs2_robotic_assets | 1 + ocs2_robotic_examples/COLCON_IGNORE | 0 ocs2_robotic_tools/COLCON_IGNORE | 0 ocs2_ros_interfaces/COLCON_IGNORE | 0 ocs2_slp/COLCON_IGNORE | 0 ocs2_sqp/COLCON_IGNORE | 0 ocs2_test_tools/COLCON_IGNORE | 0 ocs2_thirdparty/COLCON_IGNORE | 0 24 files changed, 10 insertions(+), 12 deletions(-) create mode 100644 ocs2/COLCON_IGNORE create mode 100644 ocs2_core/COLCON_IGNORE create mode 100644 ocs2_ddp/COLCON_IGNORE create mode 100644 ocs2_doc/COLCON_IGNORE create mode 100644 ocs2_frank_wolfe/COLCON_IGNORE create mode 100644 ocs2_ipm/COLCON_IGNORE create mode 100644 ocs2_mpc/COLCON_IGNORE create mode 100644 ocs2_mpcnet/COLCON_IGNORE create mode 100644 ocs2_msgs/COLCON_IGNORE create mode 100644 ocs2_oc/COLCON_IGNORE create mode 100644 ocs2_ocs2/COLCON_IGNORE create mode 100644 ocs2_perceptive/COLCON_IGNORE create mode 100644 ocs2_pinocchio/COLCON_IGNORE create mode 100644 ocs2_python_interface/COLCON_IGNORE create mode 100644 ocs2_raisim/COLCON_IGNORE create mode 160000 ocs2_robotic_assets create mode 100644 ocs2_robotic_examples/COLCON_IGNORE create mode 100644 ocs2_robotic_tools/COLCON_IGNORE create mode 100644 ocs2_ros_interfaces/COLCON_IGNORE create mode 100644 ocs2_slp/COLCON_IGNORE create mode 100644 ocs2_sqp/COLCON_IGNORE create mode 100644 ocs2_test_tools/COLCON_IGNORE create mode 100644 ocs2_thirdparty/COLCON_IGNORE diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index 50b9cc720..94e2b13a3 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -17,7 +17,7 @@ jobs: # environment: regular Ubuntu with a vanilla ROS container runs-on: [self-hosted, Linux, X64] - container: ros:noetic + container: ros:humble steps: - name: Environment Info @@ -30,13 +30,14 @@ jobs: shell: bash run: | apt-get update - source /opt/ros/noetic/setup.bash + source /opt/ros/humble/setup.bash DEBIAN_FRONTEND=noninteractive apt-get install keyboard-configuration --assume-yes apt-get install libglpk-dev -y apt-get install libmpfr-dev -y - apt-get install python3-catkin-tools -y - apt-get install ros-noetic-cv-bridge -y + apt-get install python3-colcon-common-extensions -y + apt-get install ros-humble-cv-bridge -y apt-get install git liburdfdom-dev liboctomap-dev libassimp-dev libeigen3-dev -y + apt-get install ros-humble-pinocchio ros-humble-hpp-fcl -y - uses: actions/checkout@v2 with: @@ -48,31 +49,26 @@ jobs: echo "Cleaning up existing src/pinocchio directory..." rm -rf src/pinocchio fi - git clone --recurse-submodules https://github.com/leggedrobotics/pinocchio.git src/pinocchio if [ -d "src/hpp-fcl" ]; then echo "Cleaning up existing src/hpp-fcl directory..." rm -rf src/hpp-fcl fi - git clone --recurse-submodules https://github.com/leggedrobotics/hpp-fcl.git src/hpp-fcl if [ -d "src/ocs2_robotic_assets" ]; then echo "Cleaning up existing src/ocs2_robotic_assets directory..." rm -rf src/ocs2_robotic_assets fi - git clone https://github.com/leggedrobotics/ocs2_robotic_assets.git src/ocs2_robotic_assets if [ -d "src/elevation_mapping_cupy" ]; then echo "Cleaning up existing src/evelation_cupy directory..." rm -rf src/elevation_mapping_cupy fi - git clone https://github.com/leggedrobotics/elevation_mapping_cupy.git src/elevation_mapping_cupy if [ -d "src/grid_map" ]; then echo "Cleaning up existing src/grid_map directory..." rm -rf src/grid_map fi - git clone https://github.com/ANYbotics/grid_map.git src/grid_map - name: Rosdep run: | - rosdep update --rosdistro noetic + rosdep update --rosdistro humble rosdep install --from-paths src --ignore-src -r -y # - name: Install RaiSim @@ -95,8 +91,9 @@ jobs: - name: Build shell: bash run: | - source /opt/ros/noetic/setup.bash - catkin build -DCMAKE_BUILD_TYPE=Release + rm -fr build install log + source /opt/ros/humble/setup.bash + colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release # - name: Test # shell: bash diff --git a/ocs2/COLCON_IGNORE b/ocs2/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_core/COLCON_IGNORE b/ocs2_core/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_ddp/COLCON_IGNORE b/ocs2_ddp/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_doc/COLCON_IGNORE b/ocs2_doc/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_frank_wolfe/COLCON_IGNORE b/ocs2_frank_wolfe/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_ipm/COLCON_IGNORE b/ocs2_ipm/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_mpc/COLCON_IGNORE b/ocs2_mpc/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_mpcnet/COLCON_IGNORE b/ocs2_mpcnet/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_msgs/COLCON_IGNORE b/ocs2_msgs/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_oc/COLCON_IGNORE b/ocs2_oc/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_ocs2/COLCON_IGNORE b/ocs2_ocs2/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_perceptive/COLCON_IGNORE b/ocs2_perceptive/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_pinocchio/COLCON_IGNORE b/ocs2_pinocchio/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_python_interface/COLCON_IGNORE b/ocs2_python_interface/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_raisim/COLCON_IGNORE b/ocs2_raisim/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_assets b/ocs2_robotic_assets new file mode 160000 index 000000000..b126d00d5 --- /dev/null +++ b/ocs2_robotic_assets @@ -0,0 +1 @@ +Subproject commit b126d00d55f1e67905c3c1516995466df6873c5c diff --git a/ocs2_robotic_examples/COLCON_IGNORE b/ocs2_robotic_examples/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_tools/COLCON_IGNORE b/ocs2_robotic_tools/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_ros_interfaces/COLCON_IGNORE b/ocs2_ros_interfaces/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_slp/COLCON_IGNORE b/ocs2_slp/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_sqp/COLCON_IGNORE b/ocs2_sqp/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_test_tools/COLCON_IGNORE b/ocs2_test_tools/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_thirdparty/COLCON_IGNORE b/ocs2_thirdparty/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb From e35395cabd59d035db92369d1bf8b7b34fce1aa9 Mon Sep 17 00:00:00 2001 From: jzhao6x <88374403+jzhao6x@users.noreply.github.com> Date: Mon, 18 Aug 2025 17:35:48 +0800 Subject: [PATCH 12/42] CI: remove ocs2_robotic_assets as git submodule (#16) * remove ocs2_robotic_assets * change git action --- .github/workflows/ros-build-test.yml | 4 ++-- ocs2_robotic_assets | 1 - 2 files changed, 2 insertions(+), 3 deletions(-) delete mode 160000 ocs2_robotic_assets diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index 94e2b13a3..72d1a3e6d 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -35,13 +35,13 @@ jobs: apt-get install libglpk-dev -y apt-get install libmpfr-dev -y apt-get install python3-colcon-common-extensions -y - apt-get install ros-humble-cv-bridge -y apt-get install git liburdfdom-dev liboctomap-dev libassimp-dev libeigen3-dev -y apt-get install ros-humble-pinocchio ros-humble-hpp-fcl -y - - uses: actions/checkout@v2 + - uses: actions/checkout@v4 with: path: src/ocs2 + submodules: false - name: Checkout dependencies run: | diff --git a/ocs2_robotic_assets b/ocs2_robotic_assets deleted file mode 160000 index b126d00d5..000000000 --- a/ocs2_robotic_assets +++ /dev/null @@ -1 +0,0 @@ -Subproject commit b126d00d55f1e67905c3c1516995466df6873c5c From ce1e8f64ca077bb53094e689ba705456b067ac13 Mon Sep 17 00:00:00 2001 From: xzhan34 Date: Tue, 19 Aug 2025 09:57:00 +0800 Subject: [PATCH 13/42] ocs2_thirdparty: porting for ros2 humble. (#12) updated ocs2_thirdparty to ros2 humble as the part of the ocs2 porting effort. Signed-off-by: Zhang, Xiaolin --- ocs2_thirdparty/CMakeLists.txt | 13 ++++++------- ocs2_thirdparty/package.xml | 10 +++++++--- 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/ocs2_thirdparty/CMakeLists.txt b/ocs2_thirdparty/CMakeLists.txt index 9217b8724..103599ad2 100644 --- a/ocs2_thirdparty/CMakeLists.txt +++ b/ocs2_thirdparty/CMakeLists.txt @@ -1,12 +1,11 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.14.4) project(ocs2_thirdparty) -find_package(catkin) +find_package(ament_cmake REQUIRED) -catkin_package( - INCLUDE_DIRS - include -) +ament_export_include_directories(include) install(DIRECTORY include/ - DESTINATION "${CATKIN_PACKAGE_INCLUDE_DESTINATION}/../") + DESTINATION include) + +ament_package() diff --git a/ocs2_thirdparty/package.xml b/ocs2_thirdparty/package.xml index 01135f0ec..8a54a6529 100644 --- a/ocs2_thirdparty/package.xml +++ b/ocs2_thirdparty/package.xml @@ -1,5 +1,6 @@ - + + ocs2_thirdparty 0.0.1 Package containing third party libraries that OCS2 uses. @@ -10,8 +11,11 @@ Check individual license files for each library - catkin - cmake_modules + ament_cmake + + + ament_cmake + From 81509b1b97955fc52edfb1c6dfbe92ffb5c5ca6f Mon Sep 17 00:00:00 2001 From: xzhan34 Date: Wed, 20 Aug 2025 16:51:46 +0800 Subject: [PATCH 14/42] enable ocs2_thirdparty in the build system. (#23) delete the ignore file which is used for skipping build in the CI. --------- Signed-off-by: Zhang, Xiaolin --- .github/CODEOWNERS | 3 ++- ocs2_thirdparty/COLCON_IGNORE | 0 2 files changed, 2 insertions(+), 1 deletion(-) delete mode 100644 ocs2_thirdparty/COLCON_IGNORE diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 267fd7a5f..1b8a3a21f 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,4 +1,5 @@ # OCS2 Intel-maintained repo default reviewers # Default Reviewers: -* @RoboticsYY @baihe-liu @AnnikaWU \ No newline at end of file +* @RoboticsYY @baihe-liu @AnnikaWU +/ocs2_thirdpary @xzhan34 diff --git a/ocs2_thirdparty/COLCON_IGNORE b/ocs2_thirdparty/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 From bf24dad4cf08c4e4a2de46866df9049b59d77cfa Mon Sep 17 00:00:00 2001 From: sunausti Date: Thu, 21 Aug 2025 02:14:05 +0000 Subject: [PATCH 15/42] ocs2_msgs: porting to ros2 humble (#21) * ocs2_msgs: porting to ros2 humble rename msgs files and msg name to follow https://design.ros2.org/articles/interface_definition.html Signed-off-by: Austin Sun --- ocs2_msgs/CMakeLists.txt | 63 +++++++++---------- ocs2_msgs/COLCON_IGNORE | 0 .../msg/{constraint.msg => Constraint.msg} | 0 ...controller_data.msg => ControllerData.msg} | 0 ...gian_metrics.msg => LagrangianMetrics.msg} | 0 ocs2_msgs/msg/ModeSchedule.msg | 4 ++ ocs2_msgs/msg/MpcFlattenedController.msg | 21 +++++++ ocs2_msgs/msg/{mpc_input.msg => MpcInput.msg} | 0 ...mpc_observation.msg => MpcObservation.msg} | 4 +- ocs2_msgs/msg/MpcPerformanceIndices.msg | 8 +++ ocs2_msgs/msg/{mpc_state.msg => MpcState.msg} | 0 ocs2_msgs/msg/MpcTargetTrajectories.msg | 6 ++ .../msg/{multiplier.msg => Multiplier.msg} | 2 +- ocs2_msgs/msg/mode_schedule.msg | 4 -- ocs2_msgs/msg/mpc_flattened_controller.msg | 21 ------- ocs2_msgs/msg/mpc_performance_indices.msg | 8 --- ocs2_msgs/msg/mpc_target_trajectories.msg | 6 -- ocs2_msgs/package.xml | 13 +++- ocs2_msgs/srv/{reset.srv => Reset.srv} | 2 +- 19 files changed, 82 insertions(+), 80 deletions(-) delete mode 100644 ocs2_msgs/COLCON_IGNORE rename ocs2_msgs/msg/{constraint.msg => Constraint.msg} (100%) rename ocs2_msgs/msg/{controller_data.msg => ControllerData.msg} (100%) rename ocs2_msgs/msg/{lagrangian_metrics.msg => LagrangianMetrics.msg} (100%) create mode 100644 ocs2_msgs/msg/ModeSchedule.msg create mode 100644 ocs2_msgs/msg/MpcFlattenedController.msg rename ocs2_msgs/msg/{mpc_input.msg => MpcInput.msg} (100%) rename ocs2_msgs/msg/{mpc_observation.msg => MpcObservation.msg} (54%) create mode 100644 ocs2_msgs/msg/MpcPerformanceIndices.msg rename ocs2_msgs/msg/{mpc_state.msg => MpcState.msg} (100%) create mode 100644 ocs2_msgs/msg/MpcTargetTrajectories.msg rename ocs2_msgs/msg/{multiplier.msg => Multiplier.msg} (70%) delete mode 100644 ocs2_msgs/msg/mode_schedule.msg delete mode 100644 ocs2_msgs/msg/mpc_flattened_controller.msg delete mode 100644 ocs2_msgs/msg/mpc_performance_indices.msg delete mode 100644 ocs2_msgs/msg/mpc_target_trajectories.msg rename ocs2_msgs/srv/{reset.srv => Reset.srv} (64%) diff --git a/ocs2_msgs/CMakeLists.txt b/ocs2_msgs/CMakeLists.txt index 4aed95b53..aa1ce0160 100644 --- a/ocs2_msgs/CMakeLists.txt +++ b/ocs2_msgs/CMakeLists.txt @@ -1,39 +1,34 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.14.4) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() project(ocs2_msgs) -find_package(catkin REQUIRED - COMPONENTS - std_msgs - message_generation -) - -add_message_files( - FILES - mpc_state.msg - mpc_input.msg - mode_schedule.msg - mpc_observation.msg - mpc_performance_indices.msg - mpc_target_trajectories.msg - controller_data.msg - mpc_flattened_controller.msg - lagrangian_metrics.msg - multiplier.msg - constraint.msg -) +find_package(rosidl_default_generators REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(std_msgs REQUIRED) -add_service_files( - FILES - reset.srv +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/MpcState.msg" + "msg/MpcInput.msg" + "msg/ModeSchedule.msg" + "msg/MpcObservation.msg" + "msg/MpcPerformanceIndices.msg" + "msg/MpcTargetTrajectories.msg" + "msg/ControllerData.msg" + "msg/MpcFlattenedController.msg" + "msg/LagrangianMetrics.msg" + "msg/Multiplier.msg" + "msg/Constraint.msg" + "srv/Reset.srv" + DEPENDENCIES + std_msgs + builtin_interfaces ) -generate_messages( - DEPENDENCIES - std_msgs -) - -catkin_package( - CATKIN_DEPENDS - std_msgs - message_runtime -) +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/ocs2_msgs/COLCON_IGNORE b/ocs2_msgs/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/ocs2_msgs/msg/constraint.msg b/ocs2_msgs/msg/Constraint.msg similarity index 100% rename from ocs2_msgs/msg/constraint.msg rename to ocs2_msgs/msg/Constraint.msg diff --git a/ocs2_msgs/msg/controller_data.msg b/ocs2_msgs/msg/ControllerData.msg similarity index 100% rename from ocs2_msgs/msg/controller_data.msg rename to ocs2_msgs/msg/ControllerData.msg diff --git a/ocs2_msgs/msg/lagrangian_metrics.msg b/ocs2_msgs/msg/LagrangianMetrics.msg similarity index 100% rename from ocs2_msgs/msg/lagrangian_metrics.msg rename to ocs2_msgs/msg/LagrangianMetrics.msg diff --git a/ocs2_msgs/msg/ModeSchedule.msg b/ocs2_msgs/msg/ModeSchedule.msg new file mode 100644 index 000000000..31ae19e00 --- /dev/null +++ b/ocs2_msgs/msg/ModeSchedule.msg @@ -0,0 +1,4 @@ +# MPC mode sequence + +float64[] event_times # event times: its size is equal to the size of phaseSequence minus one +int8[] mode_sequence # mode sequence: e.g., for a quadrupedal robot, it is in the set {0, 1,..., 15} \ No newline at end of file diff --git a/ocs2_msgs/msg/MpcFlattenedController.msg b/ocs2_msgs/msg/MpcFlattenedController.msg new file mode 100644 index 000000000..d922f707e --- /dev/null +++ b/ocs2_msgs/msg/MpcFlattenedController.msg @@ -0,0 +1,21 @@ +# Flattened controller: A serialized controller + +# define controllerType Enum values +uint8 CONTROLLER_UNKNOWN=0 # safety mechanism: message initalization to zero +uint8 CONTROLLER_FEEDFORWARD=1 +uint8 CONTROLLER_LINEAR=2 + +uint8 controller_type # what type of controller is this + +MpcObservation init_observation # plan initial observation + +MpcTargetTrajectories plan_target_trajectories # target trajectory in cost function +MpcState[] state_trajectory # optimized state trajectory from planner +MpcInput[] input_trajectory # optimized input trajectory from planner +float64[] time_trajectory # time trajectory for stateTrajectory and inputTrajectory +uint16[] post_event_indices # array of indices indicating the index of post-event time in the trajectories +ModeSchedule mode_schedule # optimal/predefined MPC mode sequence and event times + +ControllerData[] data # the actual payload from flatten method: one vector of data per time step + +MpcPerformanceIndices performance_indices # solver performance indices diff --git a/ocs2_msgs/msg/mpc_input.msg b/ocs2_msgs/msg/MpcInput.msg similarity index 100% rename from ocs2_msgs/msg/mpc_input.msg rename to ocs2_msgs/msg/MpcInput.msg diff --git a/ocs2_msgs/msg/mpc_observation.msg b/ocs2_msgs/msg/MpcObservation.msg similarity index 54% rename from ocs2_msgs/msg/mpc_observation.msg rename to ocs2_msgs/msg/MpcObservation.msg index 675ec39b8..7669246a4 100644 --- a/ocs2_msgs/msg/mpc_observation.msg +++ b/ocs2_msgs/msg/MpcObservation.msg @@ -1,5 +1,5 @@ # MPC observation float64 time # Current time -mpc_state state # Current state -mpc_input input # Current input +MpcState state # Current state +MpcInput input # Current input int8 mode # Current mode diff --git a/ocs2_msgs/msg/MpcPerformanceIndices.msg b/ocs2_msgs/msg/MpcPerformanceIndices.msg new file mode 100644 index 000000000..9897f0aaa --- /dev/null +++ b/ocs2_msgs/msg/MpcPerformanceIndices.msg @@ -0,0 +1,8 @@ +# MPC performance indices +float32 init_time +float32 merit +float32 cost +float32 dynamics_violation_sse +float32 equality_constraints_sse +float32 equality_lagrangian +float32 inequality_lagrangian diff --git a/ocs2_msgs/msg/mpc_state.msg b/ocs2_msgs/msg/MpcState.msg similarity index 100% rename from ocs2_msgs/msg/mpc_state.msg rename to ocs2_msgs/msg/MpcState.msg diff --git a/ocs2_msgs/msg/MpcTargetTrajectories.msg b/ocs2_msgs/msg/MpcTargetTrajectories.msg new file mode 100644 index 000000000..7553046f1 --- /dev/null +++ b/ocs2_msgs/msg/MpcTargetTrajectories.msg @@ -0,0 +1,6 @@ +# MPC target trajectories + +float64[] time_trajectory # MPC target time trajectory +MpcState[] state_trajectory # MPC target state trajectory +MpcInput[] input_trajectory # MPC target input trajectory + diff --git a/ocs2_msgs/msg/multiplier.msg b/ocs2_msgs/msg/Multiplier.msg similarity index 70% rename from ocs2_msgs/msg/multiplier.msg rename to ocs2_msgs/msg/Multiplier.msg index 69fdb65e7..f085ec553 100644 --- a/ocs2_msgs/msg/multiplier.msg +++ b/ocs2_msgs/msg/Multiplier.msg @@ -2,4 +2,4 @@ float32 time float32 penalty -float32[] lagrangian \ No newline at end of file +float32[] lagrangian diff --git a/ocs2_msgs/msg/mode_schedule.msg b/ocs2_msgs/msg/mode_schedule.msg deleted file mode 100644 index 79bb61ce6..000000000 --- a/ocs2_msgs/msg/mode_schedule.msg +++ /dev/null @@ -1,4 +0,0 @@ -# MPC mode sequence - -float64[] eventTimes # event times: its size is equal to the size of phaseSequence minus one -int8[] modeSequence # mode sequence: e.g., for a quadrupedal robot, it is in the set {0, 1,..., 15} \ No newline at end of file diff --git a/ocs2_msgs/msg/mpc_flattened_controller.msg b/ocs2_msgs/msg/mpc_flattened_controller.msg deleted file mode 100644 index d1dff6f1c..000000000 --- a/ocs2_msgs/msg/mpc_flattened_controller.msg +++ /dev/null @@ -1,21 +0,0 @@ -# Flattened controller: A serialized controller - -# define controllerType Enum values -uint8 CONTROLLER_UNKNOWN=0 # safety mechanism: message initalization to zero -uint8 CONTROLLER_FEEDFORWARD=1 -uint8 CONTROLLER_LINEAR=2 - -uint8 controllerType # what type of controller is this - -mpc_observation initObservation # plan initial observation - -mpc_target_trajectories planTargetTrajectories # target trajectory in cost function -mpc_state[] stateTrajectory # optimized state trajectory from planner -mpc_input[] inputTrajectory # optimized input trajectory from planner -float64[] timeTrajectory # time trajectory for stateTrajectory and inputTrajectory -uint16[] postEventIndices # array of indices indicating the index of post-event time in the trajectories -mode_schedule modeSchedule # optimal/predefined MPC mode sequence and event times - -controller_data[] data # the actual payload from flatten method: one vector of data per time step - -mpc_performance_indices performanceIndices # solver performance indices diff --git a/ocs2_msgs/msg/mpc_performance_indices.msg b/ocs2_msgs/msg/mpc_performance_indices.msg deleted file mode 100644 index 2eb313e57..000000000 --- a/ocs2_msgs/msg/mpc_performance_indices.msg +++ /dev/null @@ -1,8 +0,0 @@ -# MPC performance indices -float32 initTime -float32 merit -float32 cost -float32 dynamicsViolationSSE -float32 equalityConstraintsSSE -float32 equalityLagrangian -float32 inequalityLagrangian diff --git a/ocs2_msgs/msg/mpc_target_trajectories.msg b/ocs2_msgs/msg/mpc_target_trajectories.msg deleted file mode 100644 index 8b375141d..000000000 --- a/ocs2_msgs/msg/mpc_target_trajectories.msg +++ /dev/null @@ -1,6 +0,0 @@ -# MPC target trajectories - -float64[] timeTrajectory # MPC target time trajectory -mpc_state[] stateTrajectory # MPC target state trajectory -mpc_input[] inputTrajectory # MPC target input trajectory - diff --git a/ocs2_msgs/package.xml b/ocs2_msgs/package.xml index 496d6563f..8a4838fba 100644 --- a/ocs2_msgs/package.xml +++ b/ocs2_msgs/package.xml @@ -1,6 +1,6 @@ - + ocs2_msgs 0.0.0 @@ -11,12 +11,19 @@ Jan Carius Ruben Grandia - catkin + ament_cmake - message_generation + rosidl_default_generators std_msgs + rosidl_default_runtime + rosidl_interface_packages + message_runtime + + ament_cmake + + diff --git a/ocs2_msgs/srv/reset.srv b/ocs2_msgs/srv/Reset.srv similarity index 64% rename from ocs2_msgs/srv/reset.srv rename to ocs2_msgs/srv/Reset.srv index a3a28420a..5d854d5db 100644 --- a/ocs2_msgs/srv/reset.srv +++ b/ocs2_msgs/srv/Reset.srv @@ -1,5 +1,5 @@ # Reset service bool reset -mpc_target_trajectories targetTrajectories +MpcTargetTrajectories target_trajectories --- bool done \ No newline at end of file From b20b6a78ab5e59c44dd650adc2297a4502c95e7e Mon Sep 17 00:00:00 2001 From: jzhao6x <88374403+jzhao6x@users.noreply.github.com> Date: Thu, 21 Aug 2025 13:50:37 +0800 Subject: [PATCH 16/42] CI: Optimize CI running time (#19) * update docker image * add dockerfile * update * update dockerfile * update docker image * update * add code owner --- .docker/Dockerfile | 19 +++++++++++++++++++ .docker/build.sh | 11 +++++++++++ .github/CODEOWNERS | 4 ++++ .github/workflows/ros-build-test.yml | 20 +++++++------------- 4 files changed, 41 insertions(+), 13 deletions(-) create mode 100644 .docker/Dockerfile create mode 100644 .docker/build.sh diff --git a/.docker/Dockerfile b/.docker/Dockerfile new file mode 100644 index 000000000..75a2546ea --- /dev/null +++ b/.docker/Dockerfile @@ -0,0 +1,19 @@ +FROM osrf/ros:humble-desktop-full-jammy + +RUN apt-get update + +RUN DEBIAN_FRONTEND=noninteractive apt-get install -y \ + keyboard-configuration --assume-yes + +RUN apt-get install -y \ + libglpk-dev \ + libmpfr-dev + +RUN apt-get install -y \ + ros-humble-pinocchio \ + ros-humble-hpp-fcl + +RUN apt-get clean \ + && rm -rf /var/lib/apt/lists/* + +CMD ["bash"] diff --git a/.docker/build.sh b/.docker/build.sh new file mode 100644 index 000000000..1c0ba159e --- /dev/null +++ b/.docker/build.sh @@ -0,0 +1,11 @@ +#!/bin/bash +# please login docker hub before run this script + +IMAGE_NAME="ros2_ocs2" +IMAGE_TAG="latest" +read -p "Enter your Docker Hub username: " DOCKER_USERNAME +echo + +docker build -t ${IMAGE_NAME}:${IMAGE_TAG} . +docker tag ${IMAGE_NAME}:${IMAGE_TAG} ${DOCKER_USERNAME}/${IMAGE_NAME}:${IMAGE_TAG} +docker push ${DOCKER_USERNAME}/${IMAGE_NAME}:${IMAGE_TAG} diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 1b8a3a21f..6c1f48297 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -2,4 +2,8 @@ # Default Reviewers: * @RoboticsYY @baihe-liu @AnnikaWU + +# code owner +/.docker @jzhao6x +/.github @jzhao6x /ocs2_thirdpary @xzhan34 diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index 72d1a3e6d..b7b2ef109 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -17,7 +17,7 @@ jobs: # environment: regular Ubuntu with a vanilla ROS container runs-on: [self-hosted, Linux, X64] - container: ros:humble + container: jiex911/ros2_ocs2:latest steps: - name: Environment Info @@ -31,12 +31,6 @@ jobs: run: | apt-get update source /opt/ros/humble/setup.bash - DEBIAN_FRONTEND=noninteractive apt-get install keyboard-configuration --assume-yes - apt-get install libglpk-dev -y - apt-get install libmpfr-dev -y - apt-get install python3-colcon-common-extensions -y - apt-get install git liburdfdom-dev liboctomap-dev libassimp-dev libeigen3-dev -y - apt-get install ros-humble-pinocchio ros-humble-hpp-fcl -y - uses: actions/checkout@v4 with: @@ -95,9 +89,9 @@ jobs: source /opt/ros/humble/setup.bash colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release - # - name: Test - # shell: bash - # run: | - # source devel_isolated/setup.bash - # catkin_make_isolated --use-ninja --merge --only-pkg-with-deps ocs2 --catkin-make-args run_tests - # catkin_test_results + - name: Test + shell: bash + run: | + source install/setup.bash + colcon test + From 611a6e2d152d0a5fd9253124c8f4b3513dd386bb Mon Sep 17 00:00:00 2001 From: sunausti Date: Thu, 21 Aug 2025 05:53:36 +0000 Subject: [PATCH 17/42] Update Code Owner ocs2_msgs and tools (#24) Signed-off-by: Austin Sun --- .github/CODEOWNERS | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 6c1f48297..6740cae99 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -7,3 +7,5 @@ /.docker @jzhao6x /.github @jzhao6x /ocs2_thirdpary @xzhan34 +/ocs2_msgs @sunausti +/ocs2_robotic_tools @sunausti From cbe285a98a85b613c15053904cbf8ad19c701137 Mon Sep 17 00:00:00 2001 From: jzhao6x <88374403+jzhao6x@users.noreply.github.com> Date: Thu, 21 Aug 2025 17:03:54 +0800 Subject: [PATCH 18/42] CI: update docker registry (#26) * update docker registry * update dns --- .docker/build.sh | 7 +++---- .github/workflows/ros-build-test.yml | 4 ++-- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/.docker/build.sh b/.docker/build.sh index 1c0ba159e..68998c73e 100644 --- a/.docker/build.sh +++ b/.docker/build.sh @@ -3,9 +3,8 @@ IMAGE_NAME="ros2_ocs2" IMAGE_TAG="latest" -read -p "Enter your Docker Hub username: " DOCKER_USERNAME -echo +IMAGE_REGISTRY="ecgfiscbuildserver1.sh.intel.com:5000" docker build -t ${IMAGE_NAME}:${IMAGE_TAG} . -docker tag ${IMAGE_NAME}:${IMAGE_TAG} ${DOCKER_USERNAME}/${IMAGE_NAME}:${IMAGE_TAG} -docker push ${DOCKER_USERNAME}/${IMAGE_NAME}:${IMAGE_TAG} +docker tag ${IMAGE_NAME}:${IMAGE_TAG} ${IMAGE_REGISTRY}/${IMAGE_NAME}:${IMAGE_TAG} +docker push ${IMAGE_REGISTRY}/${IMAGE_NAME}:${IMAGE_TAG} diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index b7b2ef109..257dc33e9 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -17,7 +17,7 @@ jobs: # environment: regular Ubuntu with a vanilla ROS container runs-on: [self-hosted, Linux, X64] - container: jiex911/ros2_ocs2:latest + container: ecgfiscbuildserver1.sh.intel.com:5000/ros2_ocs2:latest steps: - name: Environment Info @@ -94,4 +94,4 @@ jobs: run: | source install/setup.bash colcon test - + colcon test-result --all --verbose From 2bd3847709f83ef4675c9293c2d7a8bb516ce091 Mon Sep 17 00:00:00 2001 From: xzhan34 Date: Fri, 22 Aug 2025 09:51:03 +0800 Subject: [PATCH 19/42] ocs2_core: porting for ros2 humble. (#14) updated this repo to ros2 hubmle as the part of the ocs2 porting effort. Signed-off-by: Zhang, Xiaolin --- .github/CODEOWNERS | 1 + ocs2_core/CMakeLists.txt | 168 +++++++++++++-------------- ocs2_core/COLCON_IGNORE | 0 ocs2_core/cmake/ocs2_cxx_flags.cmake | 12 +- ocs2_core/package.xml | 19 ++- 5 files changed, 107 insertions(+), 93 deletions(-) delete mode 100644 ocs2_core/COLCON_IGNORE diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 6740cae99..3812e5457 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -9,3 +9,4 @@ /ocs2_thirdpary @xzhan34 /ocs2_msgs @sunausti /ocs2_robotic_tools @sunausti +/ocs2_core @xzhan34 diff --git a/ocs2_core/CMakeLists.txt b/ocs2_core/CMakeLists.txt index 5bb261364..42cc7278d 100644 --- a/ocs2_core/CMakeLists.txt +++ b/ocs2_core/CMakeLists.txt @@ -1,18 +1,18 @@ -cmake_minimum_required(VERSION 3.0.2) -project(ocs2_core) +cmake_minimum_required(VERSION 3.14.4) +project(ocs2_core LANGUAGES CXX) -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ocs2_thirdparty -) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) +find_package(ament_cmake REQUIRED) +find_package(ocs2_thirdparty REQUIRED) find_package(Boost REQUIRED COMPONENTS system filesystem log_setup log ) -find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3) # Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) @@ -31,41 +31,21 @@ find_package(OpenMP REQUIRED) include(cmake/ocs2_cxx_flags.cmake) message(STATUS "OCS2_CXX_FLAGS: " ${OCS2_CXX_FLAGS}) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - test/include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ocs2_thirdparty - DEPENDS - Boost - OpenMP_CXX - Threads - CFG_EXTRAS - ocs2_cxx_flags.cmake -) - ########### ## Build ## ########### -include_directories( - include - test/include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} +set(dependencies + ocs2_thirdparty + Boost + Eigen3 + OpenMP + OpenMP_CXX + Threads ) # Declare a C++ library -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED src/Types.cpp src/augmented_lagrangian/AugmentedLagrangian.cpp src/augmented_lagrangian/StateAugmentedLagrangian.cpp @@ -150,19 +130,28 @@ add_library(${PROJECT_NAME} src/penalties/penalties/SquaredHingePenalty.cpp src/thread_support/ThreadPool.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - OpenMP::OpenMP_CXX - Threads::Threads +set(includes_${PROJECT_NAME} + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) +target_include_directories(${PROJECT_NAME} PUBLIC + ${includes_${PROJECT_NAME}} ) +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} +) + target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) add_executable(${PROJECT_NAME}_lintTarget src/lintTarget.cpp ) -target_link_libraries(${PROJECT_NAME}_lintTarget - ${Boost_LIBRARIES} +target_include_directories(${PROJECT_NAME}_lintTarget PUBLIC + ${includes_${PROJECT_NAME}} +) +ament_target_dependencies(${PROJECT_NAME}_lintTarget + ${dependencies} ) ######################### @@ -186,20 +175,23 @@ endif(cmake_clang_tools_FOUND) ############# install( - TARGETS - ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME}) -install(DIRECTORY test/include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY test/include/ + DESTINATION include/${PROJECT_NAME} ) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${dependencies}) + ############# ## Testing ## ############# @@ -210,27 +202,40 @@ install(DIRECTORY test/include/${PROJECT_NAME}/ ## to see the summary of unit test results run ## $ catkin_test_results ../../../build/ocs2_core -catkin_add_gtest(test_control +find_package(gtest_vendor REQUIRED) +find_package(ament_cmake_gtest REQUIRED) + +set(test_includes_${PROJECT_NAME} + ${includes_${PROJECT_NAME}} + $ +) + +function(ament_add_gtest_wrapper test_name) + ament_add_gtest(${test_name} ${ARGN}) + target_include_directories(${test_name} PUBLIC + ${test_includes_${PROJECT_NAME}} + ) +endfunction() + +ament_add_gtest_wrapper(test_control test/control/testLinearController.cpp test/control/testFeedforwardController.cpp ) target_link_libraries(test_control ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(initialization_unittest +ament_add_gtest_wrapper(initialization_unittest test/initialization/InitializationTest.cpp ) target_link_libraries(initialization_unittest ${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} gtest_main ) -catkin_add_gtest(test_integration +ament_add_gtest_wrapper(test_integration test/integration/testSensitivityIntegrator.cpp test/integration/IntegrationTest.cpp test/integration/testRungeKuttaDormandPrince5.cpp @@ -238,21 +243,19 @@ catkin_add_gtest(test_integration ) target_link_libraries(test_integration ${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} gtest_main ) -catkin_add_gtest(interpolation_unittest +ament_add_gtest_wrapper(interpolation_unittest test/misc/testInterpolation.cpp ) target_link_libraries(interpolation_unittest ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(${PROJECT_NAME}_cppadcg +ament_add_gtest_wrapper(${PROJECT_NAME}_cppadcg test/cppad_cg/testCppADCG_dynamics.cpp test/cppad_cg/testSparsityHelpers.cpp test/cppad_cg/testCppAdInterface.cpp @@ -260,21 +263,19 @@ catkin_add_gtest(${PROJECT_NAME}_cppadcg target_link_libraries(${PROJECT_NAME}_cppadcg ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} -lm -ldl gtest_main ) -catkin_add_gtest(test_transferfunctionbase +ament_add_gtest_wrapper(test_transferfunctionbase test/dynamics/testTransferfunctionBase.cpp ) target_link_libraries(test_transferfunctionbase ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(${PROJECT_NAME}_loopshaping +ament_add_gtest_wrapper(${PROJECT_NAME}_loopshaping test/loopshaping/testLoopshapingConfiguration.cpp test/loopshaping/testLoopshapingAugmentedLagrangian.cpp test/loopshaping/testLoopshapingConstraint.cpp @@ -288,12 +289,11 @@ catkin_add_gtest(${PROJECT_NAME}_loopshaping target_link_libraries(${PROJECT_NAME}_loopshaping ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} -lstdc++fs gtest_main ) -catkin_add_gtest(${PROJECT_NAME}_test_misc +ament_add_gtest_wrapper(${PROJECT_NAME}_test_misc test/misc/testInterpolation.cpp test/misc/testLinearAlgebra.cpp test/misc/testLogging.cpp @@ -303,21 +303,19 @@ catkin_add_gtest(${PROJECT_NAME}_test_misc target_link_libraries(${PROJECT_NAME}_test_misc ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_dynamics +ament_add_gtest_wrapper(test_dynamics test/dynamics/testSystemDynamicsLinearizer.cpp test/dynamics/testSystemDynamicsPreComputation.cpp ) target_link_libraries(test_dynamics ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_cost +ament_add_gtest_wrapper(test_cost test/cost/testCostCollection.cpp test/cost/testCostCppAd.cpp test/cost/testQuadraticCostFunction.cpp @@ -325,11 +323,11 @@ catkin_add_gtest(test_cost target_link_libraries(test_cost ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} gtest_main + -ldl ) -catkin_add_gtest(test_constraint +ament_add_gtest_wrapper(test_constraint test/constraint/testConstraintCollection.cpp test/constraint/testConstraintCppAd.cpp test/constraint/testLinearConstraint.cpp @@ -337,58 +335,54 @@ catkin_add_gtest(test_constraint target_link_libraries(test_constraint ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} gtest_main + -ldl ) -catkin_add_gtest(test_metrics +ament_add_gtest_wrapper(test_metrics test/model_data/testMetrics.cpp ) target_link_libraries(test_metrics ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_multiplier +ament_add_gtest_wrapper(test_multiplier test/model_data/testMultiplier.cpp ) target_link_libraries(test_multiplier ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main + -ldl ) -catkin_add_gtest(test_ModelData +ament_add_gtest_wrapper(test_ModelData test/model_data/testModelData.cpp ) target_link_libraries(test_ModelData ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_ModeSchedule +ament_add_gtest_wrapper(test_ModeSchedule test/reference/testModeSchedule.cpp ) target_link_libraries(test_ModeSchedule ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_softConstraint +ament_add_gtest_wrapper(test_softConstraint test/soft_constraint/testSoftConstraint.cpp test/soft_constraint/testDoubleSidedPenalty.cpp ) target_link_libraries(test_softConstraint ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(${PROJECT_NAME}_test_thread_support +ament_add_gtest_wrapper(${PROJECT_NAME}_test_thread_support test/thread_support/testBufferedValue.cpp test/thread_support/testSynchronized.cpp test/thread_support/testThreadPool.cpp @@ -396,17 +390,17 @@ catkin_add_gtest(${PROJECT_NAME}_test_thread_support target_link_libraries(${PROJECT_NAME}_test_thread_support ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(${PROJECT_NAME}_test_core +ament_add_gtest_wrapper(${PROJECT_NAME}_test_core test/testPrecomputation.cpp test/testTypes.cpp ) target_link_libraries(${PROJECT_NAME}_test_core ${PROJECT_NAME} ${Boost_LIBRARIES} - ${catkin_LIBRARIES} gtest_main ) + +ament_package() diff --git a/ocs2_core/COLCON_IGNORE b/ocs2_core/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/ocs2_core/cmake/ocs2_cxx_flags.cmake b/ocs2_core/cmake/ocs2_cxx_flags.cmake index 7f38eb333..88d6571b6 100644 --- a/ocs2_core/cmake/ocs2_cxx_flags.cmake +++ b/ocs2_core/cmake/ocs2_cxx_flags.cmake @@ -22,5 +22,13 @@ list(APPEND OCS2_CXX_FLAGS ) # Cpp standard version -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wno-sign-compare -Wno-range-loop-construct -Wno-ignored-qualifiers) + add_compile_options(-Wno-unused-parameter -Wno-unused-variable -Wno-range-loop-construct) + add_compile_options(-Wno-implicit-fallthrough -Wno-reorder -Wno-redundant-move -Wno-missing-field-initializers) +endif() \ No newline at end of file diff --git a/ocs2_core/package.xml b/ocs2_core/package.xml index 4d1e695a3..337a1d0a8 100644 --- a/ocs2_core/package.xml +++ b/ocs2_core/package.xml @@ -1,5 +1,6 @@ - + + ocs2_core 0.0.0 The ocs2_core package @@ -10,10 +11,20 @@ BSD - catkin - cmake_modules - cmake_clang_tools + ament_cmake + eigen3_cmake_module + eigen3_cmake_module + eigen + boost ocs2_thirdparty + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + + From ecb890d1934f679782feb28fe8b360560986a476 Mon Sep 17 00:00:00 2001 From: jzhao6x <88374403+jzhao6x@users.noreply.github.com> Date: Mon, 25 Aug 2025 09:19:01 +0800 Subject: [PATCH 20/42] CI: add ocs2_robotic_assets (#27) * add ocs2_robotic_assets * update * update checkout * update * update --- .github/workflows/ros-build-test.yml | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index 257dc33e9..1bfda164f 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -60,6 +60,13 @@ jobs: rm -rf src/grid_map fi + - uses: actions/checkout@v4 + with: + repository: intel-sandbox/ocs2_robotic_assets + ref: ros2 + path: src/ocs2_robotic_assets + token: ${{ secrets.GH_PAT }} + - name: Rosdep run: | rosdep update --rosdistro humble From 86b83b56d28e6719e2288b4293e69b9e2d48d02b Mon Sep 17 00:00:00 2001 From: xzhan34 Date: Mon, 25 Aug 2025 09:58:06 +0800 Subject: [PATCH 21/42] ocs2_oc: porting for ros2 humble. (#28) updated this repo to ros2 hubmle as the part of the ocs2 porting effort. Signed-off-by: Zhang, Xiaolin --- .github/CODEOWNERS | 1 + ocs2_oc/CMakeLists.txt | 155 +++++++++--------- ocs2_oc/COLCON_IGNORE | 0 ocs2_oc/package.xml | 22 ++- .../src/rollout/PerformanceIndicesRollout.cpp | 1 + ocs2_oc/test/testChangeOfInputVariables.cpp | 2 +- 6 files changed, 98 insertions(+), 83 deletions(-) delete mode 100644 ocs2_oc/COLCON_IGNORE diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 3812e5457..fae89ff91 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -10,3 +10,4 @@ /ocs2_msgs @sunausti /ocs2_robotic_tools @sunausti /ocs2_core @xzhan34 +/ocs2_oc @xzhan34 diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index 4ba703f79..84fb642c4 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -1,49 +1,40 @@ -cmake_minimum_required(VERSION 3.0.2) -project(ocs2_oc) +cmake_minimum_required(VERSION 3.14.4) +project(ocs2_oc LANGUAGES CXX) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wno-extra -Wpedantic) + add_compile_options(-Wno-sign-compare -Wno-range-loop-construct -Wno-ignored-qualifiers -Wno-unused-function) + add_compile_options(-Wno-unused-parameter -Wno-unused-variable -Wno-unused-but-set-variable) + add_compile_options(-Wno-implicit-fallthrough -Wno-reorder -Wno-redundant-move -Wno-missing-field-initializers) +endif() ## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ocs2_core - # ocs2_qp_solver -) +find_package(ament_cmake REQUIRED) +find_package(ocs2_thirdparty REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(OpenMP REQUIRED) find_package(Boost REQUIRED COMPONENTS system filesystem + log_setup ) - -find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3) # Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - test/include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ocs2_core - DEPENDS - Boost -) - ########### ## Build ## ########### -include_directories( - include - test/include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} +set(dependencies + Boost + Eigen3 + ocs2_core ) add_library(${PROJECT_NAME} @@ -80,14 +71,27 @@ add_library(${PROJECT_NAME} src/search_strategy/FilterLinesearch.cpp src/trajectory_adjustment/TrajectorySpreading.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +set(includes_${PROJECT_NAME} + $ + $ +) +target_include_directories(${PROJECT_NAME} PUBLIC + ${includes_${PROJECT_NAME}} +) +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) add_executable(${PROJECT_NAME}_lintTarget src/lintTarget.cpp ) +target_include_directories(${PROJECT_NAME}_lintTarget PUBLIC + ${includes_${PROJECT_NAME}} +) +ament_target_dependencies(${PROJECT_NAME}_lintTarget + ${dependencies} +) ######################### ### CLANG TOOLING ### @@ -109,17 +113,22 @@ endif(cmake_clang_tools_FOUND) install( TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} ) -install(DIRECTORY test/include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY test/include/ + DESTINATION include/${PROJECT_NAME} ) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${dependencies}) + ############# ## Testing ## ############# @@ -130,83 +139,75 @@ install(DIRECTORY test/include/${PROJECT_NAME}/ ## to see the summary of unit test results run ## $ catkin_test_results ../../../build/ocs2_oc -catkin_add_gtest(test_${PROJECT_NAME}_multiple_shooting +find_package(gtest_vendor REQUIRED) +find_package(ament_cmake_gtest REQUIRED) + +set(test_includes_${PROJECT_NAME} + ${includes_${PROJECT_NAME}} + $ +) + +function(ament_add_gtest_wrapper test_name) + ament_add_gtest(${test_name} ${ARGN}) + target_include_directories(${test_name} PUBLIC + ${test_includes_${PROJECT_NAME}} + ) +endfunction() + +ament_add_gtest_wrapper(test_${PROJECT_NAME}_multiple_shooting test/multiple_shooting/testProjectionMultiplierCoefficients.cpp test/multiple_shooting/testTranscriptionMetrics.cpp test/multiple_shooting/testTranscriptionPerformanceIndex.cpp ) -add_dependencies(test_${PROJECT_NAME}_multiple_shooting - ${catkin_EXPORTED_TARGETS} +ament_target_dependencies(test_${PROJECT_NAME}_multiple_shooting + ${dependencies} ) target_link_libraries(test_${PROJECT_NAME}_multiple_shooting ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main + -ldl ) -catkin_add_gtest(test_${PROJECT_NAME}_data +ament_add_gtest_wrapper(test_${PROJECT_NAME}_data test/oc_data/testTimeDiscretization.cpp ) -add_dependencies(test_${PROJECT_NAME}_data - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(test_${PROJECT_NAME}_data ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main ) -catkin_add_gtest(test_${PROJECT_NAME}_rollout +ament_add_gtest_wrapper(test_${PROJECT_NAME}_rollout test/rollout/testTimeTriggeredRollout.cpp test/rollout/testStateTriggeredRollout.cpp ) -add_dependencies(test_${PROJECT_NAME}_rollout - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(test_${PROJECT_NAME}_rollout - ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main + ${PROJECT_NAME} ${ocs2_core_TARGETS} ) -catkin_add_gtest(test_change_of_variables +ament_add_gtest_wrapper(test_change_of_variables test/testChangeOfInputVariables.cpp ) -add_dependencies(test_change_of_variables - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(test_change_of_variables ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main ) -catkin_add_gtest(test_trajectory_spreading +ament_add_gtest_wrapper(test_trajectory_spreading test/trajectory_adjustment/TrajectorySpreadingTest.cpp ) -add_dependencies(test_trajectory_spreading - ${catkin_EXPORTED_TARGETS} -) target_link_libraries(test_trajectory_spreading ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main ) -catkin_add_gtest(test_ocp_to_kkt +ament_add_gtest_wrapper(test_ocp_to_kkt test/oc_problem/testOcpToKkt.cpp ) target_link_libraries(test_ocp_to_kkt ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main ) -catkin_add_gtest(test_precondition +ament_add_gtest_wrapper(test_precondition test/precondition/testPrecondition.cpp ) target_link_libraries(test_precondition ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main ) + +ament_package() \ No newline at end of file diff --git a/ocs2_oc/COLCON_IGNORE b/ocs2_oc/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/ocs2_oc/package.xml b/ocs2_oc/package.xml index 00f0fbbfc..bb4cfbd33 100644 --- a/ocs2_oc/package.xml +++ b/ocs2_oc/package.xml @@ -1,5 +1,6 @@ - + + ocs2_oc 0.0.0 The ocs2_oc package @@ -10,10 +11,21 @@ BSD - catkin - cmake_modules - cmake_clang_tools + ament_cmake + eigen3_cmake_module + eigen3_cmake_module + + eigen + boost + ocs2_thirdparty ocs2_core - + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + diff --git a/ocs2_oc/src/rollout/PerformanceIndicesRollout.cpp b/ocs2_oc/src/rollout/PerformanceIndicesRollout.cpp index f2442d8d7..5815876d1 100644 --- a/ocs2_oc/src/rollout/PerformanceIndicesRollout.cpp +++ b/ocs2_oc/src/rollout/PerformanceIndicesRollout.cpp @@ -27,6 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ +#include #include #include diff --git a/ocs2_oc/test/testChangeOfInputVariables.cpp b/ocs2_oc/test/testChangeOfInputVariables.cpp index f7306587b..c11849cf7 100644 --- a/ocs2_oc/test/testChangeOfInputVariables.cpp +++ b/ocs2_oc/test/testChangeOfInputVariables.cpp @@ -109,7 +109,7 @@ TEST(quadratic_change_of_input_variables, withPx) { // Evaluate and compare const scalar_t unprojected = evaluate(quadratic, dx, Pu * du_tilde + Px * dx); const scalar_t projected = evaluate(quadraticProjected, dx, du_tilde); - ASSERT_DOUBLE_EQ(unprojected, projected); + ASSERT_NEAR(unprojected, projected, 1e-10); } TEST(quadratic_change_of_input_variables, withU0) { From 3606263d4275c02bd955ce39e97bf49e39e1dff4 Mon Sep 17 00:00:00 2001 From: "Yu, Yan" Date: Mon, 25 Aug 2025 11:37:37 +0800 Subject: [PATCH 22/42] Fix ocs2_core compile warnings (#31) --- ocs2_core/cmake/ocs2_cxx_flags.cmake | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ocs2_core/cmake/ocs2_cxx_flags.cmake b/ocs2_core/cmake/ocs2_cxx_flags.cmake index 88d6571b6..11ecf95e8 100644 --- a/ocs2_core/cmake/ocs2_cxx_flags.cmake +++ b/ocs2_core/cmake/ocs2_cxx_flags.cmake @@ -29,6 +29,6 @@ endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) add_compile_options(-Wno-sign-compare -Wno-range-loop-construct -Wno-ignored-qualifiers) - add_compile_options(-Wno-unused-parameter -Wno-unused-variable -Wno-range-loop-construct) + add_compile_options(-Wno-unused-parameter -Wno-unused-variable -Wno-range-loop-construct -Wno-empty-body -Wno-maybe-uninitialized) add_compile_options(-Wno-implicit-fallthrough -Wno-reorder -Wno-redundant-move -Wno-missing-field-initializers) -endif() \ No newline at end of file +endif() From 32b7c90699bea2186b299d6934930c70d6d3f352 Mon Sep 17 00:00:00 2001 From: sunausti Date: Mon, 25 Aug 2025 08:30:36 +0000 Subject: [PATCH 23/42] ocs2_robotic_tools: porting to ros2 humble (#25) * ocs2_robotic_tools: porting to ros2 humble --------- Signed-off-by: Austin Sun --- ocs2_oc/CMakeLists.txt | 4 +- ocs2_robotic_tools/CMakeLists.txt | 125 +++++++++++++++++------------- ocs2_robotic_tools/COLCON_IGNORE | 0 ocs2_robotic_tools/package.xml | 27 ++++--- 4 files changed, 92 insertions(+), 64 deletions(-) delete mode 100644 ocs2_robotic_tools/COLCON_IGNORE diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index 84fb642c4..7c3b76a0e 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -37,7 +37,7 @@ set(dependencies ocs2_core ) -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED src/approximate_model/ChangeOfInputVariables.cpp src/approximate_model/LinearQuadraticApproximator.cpp src/multiple_shooting/Helpers.cpp @@ -210,4 +210,4 @@ target_link_libraries(test_precondition ${PROJECT_NAME} ) -ament_package() \ No newline at end of file +ament_package() diff --git a/ocs2_robotic_tools/CMakeLists.txt b/ocs2_robotic_tools/CMakeLists.txt index 6c560fda9..37441564e 100644 --- a/ocs2_robotic_tools/CMakeLists.txt +++ b/ocs2_robotic_tools/CMakeLists.txt @@ -1,57 +1,69 @@ -cmake_minimum_required(VERSION 3.0.2) -project(ocs2_robotic_tools) +cmake_minimum_required(VERSION 3.14.4) +project(ocs2_robotic_tools LANGUAGES CXX) -# Generate compile_commands.json for clang tools -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() -find_package(catkin REQUIRED COMPONENTS - ocs2_core - ocs2_oc -) - -find_package(Eigen3 3.3 REQUIRED NO_MODULE) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -################################### -## catkin specific configuration ## -################################### +# Generate compile_commands.json for clang tools +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ocs2_core - ocs2_oc - DEPENDS +## Find ament macros and libraries +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(Boost REQUIRED COMPONENTS + system + filesystem + log_setup + log ) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) ########### ## Build ## ########### -include_directories( - include +set(includes_${PROJECT_NAME} + $ + $ ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} ) -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED src/common/RotationTransforms.cpp src/common/LoopshapingRobotInterface.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} + +target_include_directories(${PROJECT_NAME} PUBLIC + ${includes_${PROJECT_NAME}} ) + target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) +# Link ament dependencies +ament_target_dependencies(${PROJECT_NAME} ocs2_core ocs2_oc Eigen3) + add_executable(${PROJECT_NAME}_lintTarget src/lintTarget.cpp ) +target_include_directories(${PROJECT_NAME}_lintTarget PUBLIC + ${includes_${PROJECT_NAME}} +) +ament_target_dependencies(${PROJECT_NAME}_lintTarget + ocs2_core + ocs2_oc + Eigen3 +) ######################### ### CLANG TOOLING ### ######################### @@ -70,35 +82,42 @@ endif(cmake_clang_tools_FOUND) ## Install ## ############# +# Install library install( TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY - include/${PROJECT_NAME}/ - DESTINATION - ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +# Install headers +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +# Export targets +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(ocs2_core ocs2_oc Eigen3) ############# ## Testing ## ############# -## Info ============================== -## to run tests, cd package folder and run -## $ catkin build -DCMAKE_BUILD_TYPE=RelWithDebInfo --this -## $ catkin run_tests --no-deps --this -## to see the summary of unit test results run -## $ catkin_test_results ../../../build/ocs2_robotic_tools - -catkin_add_gtest(rotation_transform_tests - test/common/TestRotationTransforms.cpp - test/common/TestRotationDerivativesTransforms.cpp -) -target_link_libraries(rotation_transform_tests - ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main -) -target_compile_options(rotation_transform_tests PRIVATE ${OCS2_CXX_FLAGS}) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + # Test rotation transforms + ament_add_gtest(rotation_transform_tests + test/common/TestRotationTransforms.cpp + test/common/TestRotationDerivativesTransforms.cpp + ) + target_link_libraries(rotation_transform_tests + ${PROJECT_NAME} + ) + target_compile_options(rotation_transform_tests PRIVATE ${OCS2_CXX_FLAGS}) + ament_target_dependencies(rotation_transform_tests ocs2_core ocs2_oc) +endif() + +ament_package() diff --git a/ocs2_robotic_tools/COLCON_IGNORE b/ocs2_robotic_tools/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/ocs2_robotic_tools/package.xml b/ocs2_robotic_tools/package.xml index b73cabd70..e34e58a45 100644 --- a/ocs2_robotic_tools/package.xml +++ b/ocs2_robotic_tools/package.xml @@ -1,5 +1,5 @@ - - + + ocs2_robotic_tools 0.0.0 The ocs2_robotic_tools package @@ -8,14 +8,23 @@ Jan Carius Ruben Grandia - TODO + BSD - catkin + ament_cmake + eigen3_cmake_module + eigen3_cmake_module - cmake_clang_tools - ocs2_core - ocs2_oc - ocs2_core - ocs2_oc + boost + eigen + ocs2_core + ocs2_oc + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + From 400b5672ab219c3224ce48f3098cf4ec3f64c521 Mon Sep 17 00:00:00 2001 From: "Yu, Yan" Date: Tue, 26 Aug 2025 09:55:29 +0800 Subject: [PATCH 24/42] Clean up (#33) * Clean up the workflow file * Clean up missing changes --- .github/workflows/ros-build-test.yml | 52 +++------------------------- ocs2_core/CMakeLists.txt | 3 +- ocs2_msgs/package.xml | 2 -- ocs2_oc/CMakeLists.txt | 3 +- ocs2_robotic_tools/CMakeLists.txt | 6 ++-- 5 files changed, 9 insertions(+), 57 deletions(-) diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index 1bfda164f..92d9d260d 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -8,15 +8,13 @@ on: jobs: build: - - # build both Debug and Release mode - #strategy: - # fail-fast: false - # matrix: - # build_type: [Release] - # environment: regular Ubuntu with a vanilla ROS container runs-on: [self-hosted, Linux, X64] + # Registry the docker image to configure /etc/docker/daemon.json as: + # { + # "insecure-registries": ["XXX.XXX.XXX.XXX:XXXX", + # "ecgfiscbuildserver1.sh.intel.com:5000"] + # } container: ecgfiscbuildserver1.sh.intel.com:5000/ros2_ocs2:latest steps: @@ -36,29 +34,6 @@ jobs: with: path: src/ocs2 submodules: false - - - name: Checkout dependencies - run: | - if [ -d "src/pinocchio" ]; then - echo "Cleaning up existing src/pinocchio directory..." - rm -rf src/pinocchio - fi - if [ -d "src/hpp-fcl" ]; then - echo "Cleaning up existing src/hpp-fcl directory..." - rm -rf src/hpp-fcl - fi - if [ -d "src/ocs2_robotic_assets" ]; then - echo "Cleaning up existing src/ocs2_robotic_assets directory..." - rm -rf src/ocs2_robotic_assets - fi - if [ -d "src/elevation_mapping_cupy" ]; then - echo "Cleaning up existing src/evelation_cupy directory..." - rm -rf src/elevation_mapping_cupy - fi - if [ -d "src/grid_map" ]; then - echo "Cleaning up existing src/grid_map directory..." - rm -rf src/grid_map - fi - uses: actions/checkout@v4 with: @@ -72,23 +47,6 @@ jobs: rosdep update --rosdistro humble rosdep install --from-paths src --ignore-src -r -y - # - name: Install RaiSim - # run: | - # git clone --depth 1 https://github.com/raisimTech/raisimLib.git -b v1.1.01 src/raisim_tech - # cd src/raisim_tech - # mkdir build - # cd build - # cmake .. -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)") - # make -j4 && checkinstall - - # - name: Install ONNX Runtime - # run: | - # wget https://github.com/microsoft/onnxruntime/releases/download/v1.7.0/onnxruntime-linux-x64-1.7.0.tgz -P tmp/microsoft - # tar xf tmp/microsoft/onnxruntime-linux-x64-1.7.0.tgz -C tmp/microsoft - # rsync -a tmp/microsoft/onnxruntime-linux-x64-1.7.0/include/ /usr/local/include/onnxruntime - # rsync -a tmp/microsoft/onnxruntime-linux-x64-1.7.0/lib/ /usr/local/lib - # rsync -a src/ocs2/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/ /usr/local/share/onnxruntime - - name: Build shell: bash run: | diff --git a/ocs2_core/CMakeLists.txt b/ocs2_core/CMakeLists.txt index 42cc7278d..ad0c8971c 100644 --- a/ocs2_core/CMakeLists.txt +++ b/ocs2_core/CMakeLists.txt @@ -12,7 +12,7 @@ find_package(Boost REQUIRED COMPONENTS log ) find_package(eigen3_cmake_module REQUIRED) -find_package(Eigen3) +find_package(Eigen3 REQUIRED) # Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) @@ -133,7 +133,6 @@ add_library(${PROJECT_NAME} SHARED set(includes_${PROJECT_NAME} $ $ - ${EIGEN3_INCLUDE_DIRS} ) target_include_directories(${PROJECT_NAME} PUBLIC ${includes_${PROJECT_NAME}} diff --git a/ocs2_msgs/package.xml b/ocs2_msgs/package.xml index 8a4838fba..b16247ba5 100644 --- a/ocs2_msgs/package.xml +++ b/ocs2_msgs/package.xml @@ -20,8 +20,6 @@ rosidl_default_runtime rosidl_interface_packages - message_runtime - ament_cmake diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index 7c3b76a0e..d30e2df86 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -22,7 +22,7 @@ find_package(Boost REQUIRED COMPONENTS log_setup ) find_package(eigen3_cmake_module REQUIRED) -find_package(Eigen3) +find_package(Eigen3 REQUIRED) # Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) @@ -81,7 +81,6 @@ target_include_directories(${PROJECT_NAME} PUBLIC ament_target_dependencies(${PROJECT_NAME} ${dependencies} ) -target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) add_executable(${PROJECT_NAME}_lintTarget src/lintTarget.cpp diff --git a/ocs2_robotic_tools/CMakeLists.txt b/ocs2_robotic_tools/CMakeLists.txt index 37441564e..6c41f599b 100644 --- a/ocs2_robotic_tools/CMakeLists.txt +++ b/ocs2_robotic_tools/CMakeLists.txt @@ -7,7 +7,8 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wno-unknown-pragmas + -Wno-unused-function -Wno-unused-parameter) endif() # Generate compile_commands.json for clang tools @@ -34,7 +35,6 @@ find_package(Eigen3 REQUIRED) set(includes_${PROJECT_NAME} $ $ - ${EIGEN3_INCLUDE_DIRS} ) add_library(${PROJECT_NAME} SHARED @@ -46,8 +46,6 @@ target_include_directories(${PROJECT_NAME} PUBLIC ${includes_${PROJECT_NAME}} ) -target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) - # Link ament dependencies ament_target_dependencies(${PROJECT_NAME} ocs2_core ocs2_oc Eigen3) From 6f2f33c5ccf386c7dbac2dafefdaddda317e686b Mon Sep 17 00:00:00 2001 From: baihe-liu Date: Wed, 27 Aug 2025 11:49:23 +0800 Subject: [PATCH 25/42] ocs2_mpc porting for ros2 humble (#29) * port ocs2_mpc * remove lint target --- ocs2_mpc/CMakeLists.txt | 119 +++++++++++++++------------------------- ocs2_mpc/COLCON_IGNORE | 0 ocs2_mpc/package.xml | 16 ++++-- 3 files changed, 56 insertions(+), 79 deletions(-) delete mode 100644 ocs2_mpc/COLCON_IGNORE diff --git a/ocs2_mpc/CMakeLists.txt b/ocs2_mpc/CMakeLists.txt index 65e2df0b8..4ffcb19c7 100644 --- a/ocs2_mpc/CMakeLists.txt +++ b/ocs2_mpc/CMakeLists.txt @@ -1,50 +1,32 @@ -cmake_minimum_required(VERSION 3.0.2) -project(ocs2_mpc) - -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ocs2_core - ocs2_oc -) +cmake_minimum_required(VERSION 3.14.4) +project(ocs2_mpc LANGUAGES CXX) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter) +endif() + +find_package(ament_cmake REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_oc REQUIRED) find_package(Boost REQUIRED COMPONENTS system filesystem -) - -find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -# Generate compile_commands.json for clang tools -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ocs2_core - ocs2_oc - DEPENDS - Boost + log_setup + log ) ########### ## Build ## ########### -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED src/LoopshapingSystemObservation.cpp src/MPC_BASE.cpp src/MPC_Settings.cpp @@ -53,31 +35,18 @@ add_library(${PROJECT_NAME} src/MPC_MRT_Interface.cpp # src/MPC_OCS2.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} -) -target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -add_executable(${PROJECT_NAME}_lintTarget - src/lintTarget.cpp -) -target_link_libraries(${PROJECT_NAME}_lintTarget - ${catkin_LIBRARIES} +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ ) -######################### -### CLANG TOOLING ### -######################### -find_package(cmake_clang_tools QUIET) -if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}_lintTarget) - add_clang_tooling( - TARGETS ${PROJECT_NAME}_lintTarget - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR +ament_target_dependencies(${PROJECT_NAME} + ocs2_core + ocs2_oc + Eigen3 + Boost ) -endif(cmake_clang_tools_FOUND) ############# ## Install ## @@ -85,24 +54,24 @@ endif(cmake_clang_tools_FOUND) install( TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) -############# -## Testing ## -############# +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + ocs2_core + ocs2_oc + Eigen3 + Boost +) -#catkin_add_gtest(testMPC_OCS2 -# test/testMPC_OCS2.cpp -#) -#target_link_libraries(testMPC_OCS2 -# ${catkin_LIBRARIES} -# ${Boost_LIBRARIES} -#) -#target_compile_options(testMPC_OCS2 PRIVATE ${OCS2_CXX_FLAGS}) +ament_package() diff --git a/ocs2_mpc/COLCON_IGNORE b/ocs2_mpc/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/ocs2_mpc/package.xml b/ocs2_mpc/package.xml index 9a4968d24..44f8d251e 100644 --- a/ocs2_mpc/package.xml +++ b/ocs2_mpc/package.xml @@ -1,5 +1,6 @@ - + + ocs2_mpc 0.0.0 Model Predictive Control for SLQ @@ -10,12 +11,19 @@ BSD3 - catkin - cmake_modules - cmake_clang_tools + ament_cmake + eigen3_cmake_module + eigen3_cmake_module + + eigen + boost ocs2_core ocs2_oc + + ament_cmake + + From 13b3523a48a0f92159bf6159995fb2937d87abb7 Mon Sep 17 00:00:00 2001 From: jzhao6x <88374403+jzhao6x@users.noreply.github.com> Date: Wed, 27 Aug 2025 13:25:43 +0800 Subject: [PATCH 26/42] CI: add MujocoRosUtils (#38) * add MujocoRosUtils --- .docker/Dockerfile | 10 +++++++++- .github/workflows/ros-build-test.yml | 24 ++++++++++++++++++++++++ 2 files changed, 33 insertions(+), 1 deletion(-) diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 75a2546ea..f939ca94e 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -7,7 +7,15 @@ RUN DEBIAN_FRONTEND=noninteractive apt-get install -y \ RUN apt-get install -y \ libglpk-dev \ - libmpfr-dev + libmpfr-dev \ + libglfw3 \ + libglfw3-dev \ + libosmesa6 \ + freeglut3-dev \ + mesa-common-dev \ + python3-pip \ + python3-wstool \ + wget RUN apt-get install -y \ ros-humble-pinocchio \ diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index 92d9d260d..df66fdbdf 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -8,6 +8,9 @@ on: jobs: build: + strategy: + matrix: + mujoco-version: [2.3.7] # environment: regular Ubuntu with a vanilla ROS container runs-on: [self-hosted, Linux, X64] # Registry the docker image to configure /etc/docker/daemon.json as: @@ -19,6 +22,7 @@ jobs: steps: - name: Environment Info + shell: bash run: | pwd uname -r @@ -43,6 +47,7 @@ jobs: token: ${{ secrets.GH_PAT }} - name: Rosdep + shell: bash run: | rosdep update --rosdistro humble rosdep install --from-paths src --ignore-src -r -y @@ -51,8 +56,27 @@ jobs: shell: bash run: | rm -fr build install log + if [ -d "src/MujocoRosUtils" ]; then + echo "Cleaning up existing src/MujocoRosUtils directory..." + rm -rf src/MujocoRosUtils + fi source /opt/ros/humble/setup.bash colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release + + - name: Download MuJoCo + shell: bash + run: | + wget https://github.com/deepmind/mujoco/releases/download/${{ matrix.mujoco-version }}/mujoco-${{ matrix.mujoco-version }}-linux-x86_64.tar.gz + tar -zxvf mujoco-${{ matrix.mujoco-version }}-linux-x86_64.tar.gz + rm -fr mujoco-${{ matrix.mujoco-version }}-linux-x86_64.tar.gz + + - name: Build MuJoCoRosUtil + shell: bash + run: | + git clone --recurse-submodules https://github.com/isri-aist/MujocoRosUtils.git src/MujocoRosUtils + rosdep install -y -r --from-paths src --ignore-src + source /opt/ros/humble/setup.bash + colcon build --packages-select mujoco_ros_utils --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DMUJOCO_ROOT_DIR=${PWD}/mujoco-${{ matrix.mujoco-version }} - name: Test shell: bash From 8956d8f560a25dbe81516dcb0a58c01ab2b4e8a9 Mon Sep 17 00:00:00 2001 From: jzhao6x <88374403+jzhao6x@users.noreply.github.com> Date: Thu, 28 Aug 2025 10:02:49 +0800 Subject: [PATCH 27/42] fix issue #41 (#42) * fix issue 41 * update --- .github/workflows/ros-build-test.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index df66fdbdf..9388e5c2b 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -60,8 +60,9 @@ jobs: echo "Cleaning up existing src/MujocoRosUtils directory..." rm -rf src/MujocoRosUtils fi + rm -fr mujoco-* source /opt/ros/humble/setup.bash - colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release + colcon build --packages-skip mujoco_ros_utils --cmake-args -DCMAKE_BUILD_TYPE=Release - name: Download MuJoCo shell: bash From d5f85ae8f359cab3df5310de418269914b55c52a Mon Sep 17 00:00:00 2001 From: "Yu, Yan" Date: Thu, 28 Aug 2025 15:41:31 +0800 Subject: [PATCH 28/42] Fix ocs2_core cmake module export (#43) --- ocs2_core/CMakeLists.txt | 18 +++++++++++------- ocs2_core/cmake/ocs2_cxx_flags.cmake | 17 ++++++----------- ocs2_mpc/CMakeLists.txt | 7 +------ ocs2_oc/CMakeLists.txt | 6 +----- ocs2_robotic_tools/CMakeLists.txt | 9 +-------- 5 files changed, 20 insertions(+), 37 deletions(-) diff --git a/ocs2_core/CMakeLists.txt b/ocs2_core/CMakeLists.txt index ad0c8971c..736e73e42 100644 --- a/ocs2_core/CMakeLists.txt +++ b/ocs2_core/CMakeLists.txt @@ -1,16 +1,20 @@ cmake_minimum_required(VERSION 3.14.4) project(ocs2_core LANGUAGES CXX) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wno-sign-compare -Wno-range-loop-construct -Wno-ignored-qualifiers) + add_compile_options(-Wno-unused-parameter -Wno-unused-variable -Wno-range-loop-construct -Wno-empty-body -Wno-maybe-uninitialized) + add_compile_options(-Wno-implicit-fallthrough -Wno-reorder -Wno-redundant-move -Wno-missing-field-initializers) +endif() + set(CMAKE_POSITION_INDEPENDENT_CODE ON) find_package(ament_cmake REQUIRED) find_package(ocs2_thirdparty REQUIRED) -find_package(Boost REQUIRED COMPONENTS - system - filesystem - log_setup - log -) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) @@ -402,4 +406,4 @@ target_link_libraries(${PROJECT_NAME}_test_core gtest_main ) -ament_package() +ament_package(CONFIG_EXTRAS "cmake/ocs2_cxx_flags.cmake") diff --git a/ocs2_core/cmake/ocs2_cxx_flags.cmake b/ocs2_core/cmake/ocs2_cxx_flags.cmake index 11ecf95e8..ec07ab290 100644 --- a/ocs2_core/cmake/ocs2_cxx_flags.cmake +++ b/ocs2_core/cmake/ocs2_cxx_flags.cmake @@ -21,14 +21,9 @@ list(APPEND OCS2_CXX_FLAGS ${OpenMP_CXX_FLAGS} ) -# Cpp standard version -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) - add_compile_options(-Wno-sign-compare -Wno-range-loop-construct -Wno-ignored-qualifiers) - add_compile_options(-Wno-unused-parameter -Wno-unused-variable -Wno-range-loop-construct -Wno-empty-body -Wno-maybe-uninitialized) - add_compile_options(-Wno-implicit-fallthrough -Wno-reorder -Wno-redundant-move -Wno-missing-field-initializers) -endif() +find_package(Boost REQUIRED COMPONENTS + system + filesystem + log_setup + log +) diff --git a/ocs2_mpc/CMakeLists.txt b/ocs2_mpc/CMakeLists.txt index 4ffcb19c7..6cee42ce9 100644 --- a/ocs2_mpc/CMakeLists.txt +++ b/ocs2_mpc/CMakeLists.txt @@ -15,12 +15,6 @@ find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) find_package(ocs2_core REQUIRED) find_package(ocs2_oc REQUIRED) -find_package(Boost REQUIRED COMPONENTS - system - filesystem - log_setup - log -) ########### ## Build ## @@ -47,6 +41,7 @@ ament_target_dependencies(${PROJECT_NAME} Eigen3 Boost ) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) ############# ## Install ## diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index d30e2df86..954bd978f 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -16,11 +16,6 @@ find_package(ament_cmake REQUIRED) find_package(ocs2_thirdparty REQUIRED) find_package(ocs2_core REQUIRED) find_package(OpenMP REQUIRED) -find_package(Boost REQUIRED COMPONENTS - system - filesystem - log_setup -) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) @@ -81,6 +76,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC ament_target_dependencies(${PROJECT_NAME} ${dependencies} ) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) add_executable(${PROJECT_NAME}_lintTarget src/lintTarget.cpp diff --git a/ocs2_robotic_tools/CMakeLists.txt b/ocs2_robotic_tools/CMakeLists.txt index 6c41f599b..9c533d833 100644 --- a/ocs2_robotic_tools/CMakeLists.txt +++ b/ocs2_robotic_tools/CMakeLists.txt @@ -18,13 +18,6 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON) find_package(ament_cmake REQUIRED) find_package(ocs2_core REQUIRED) find_package(ocs2_oc REQUIRED) -find_package(Boost REQUIRED COMPONENTS - system - filesystem - log_setup - log -) - find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) @@ -48,7 +41,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC # Link ament dependencies ament_target_dependencies(${PROJECT_NAME} ocs2_core ocs2_oc Eigen3) - +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) add_executable(${PROJECT_NAME}_lintTarget src/lintTarget.cpp From 18b59190aa001d74c762ae7e647211055c771f89 Mon Sep 17 00:00:00 2001 From: Nisyhaal Date: Thu, 28 Aug 2025 16:37:44 +0800 Subject: [PATCH 29/42] ocs2_qp_solver: porting to ros2 humble (#36) * Revise cmake and package * Update CMakeLists.txt * Update package.xml --- ocs2_test_tools/COLCON_IGNORE | 0 ocs2_test_tools/ocs2_qp_solver/CMakeLists.txt | 128 +++++++++++------- ocs2_test_tools/ocs2_qp_solver/package.xml | 16 ++- 3 files changed, 91 insertions(+), 53 deletions(-) delete mode 100644 ocs2_test_tools/COLCON_IGNORE diff --git a/ocs2_test_tools/COLCON_IGNORE b/ocs2_test_tools/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/ocs2_test_tools/ocs2_qp_solver/CMakeLists.txt b/ocs2_test_tools/ocs2_qp_solver/CMakeLists.txt index 800dfc775..26e180167 100644 --- a/ocs2_test_tools/ocs2_qp_solver/CMakeLists.txt +++ b/ocs2_test_tools/ocs2_qp_solver/CMakeLists.txt @@ -1,66 +1,70 @@ -cmake_minimum_required(VERSION 3.0.2) -project(ocs2_qp_solver) +cmake_minimum_required(VERSION 3.14.4) +project(ocs2_qp_solver LANGUAGES CXX) -find_package(catkin REQUIRED COMPONENTS - ocs2_core - ocs2_oc -) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() -find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -# Generate compile_commands.json for clang tools +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - test/include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ocs2_core - ocs2_oc - DEPENDS -) +############### +## Find Packages +############### + +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) -include_directories( - include - test/include - ${EIGEN3_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} + +###################### +## Library Definition +###################### + +set(dependencies + ocs2_core + ocs2_oc + Eigen3 ) -# Declare a C++ library -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED src/Ocs2QpSolver.cpp src/QpDiscreteTranscription.cpp src/QpSolver.cpp src/QpTrajectories.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ ) + target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) -######################### -### CLANG TOOLING ### -######################### +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} +) + +###################### +## Clang Tooling (optional) +###################### find_package(cmake_clang_tools QUIET) if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}) + message(STATUS "Run clang tooling for target ${PROJECT_NAME}") add_clang_tooling( - TARGETS - ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test/include + TARGETS ${PROJECT_NAME} + SOURCE_DIRS + ${CMAKE_CURRENT_SOURCE_DIR}/src + ${CMAKE_CURRENT_SOURCE_DIR}/include + ${CMAKE_CURRENT_SOURCE_DIR}/test/include CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include CF_WERROR ) -endif(cmake_clang_tools_FOUND) +endif() ############# ## Install ## @@ -68,28 +72,50 @@ endif(cmake_clang_tools_FOUND) install( TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) -install(DIRECTORY test/include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY test/include/ + DESTINATION include/${PROJECT_NAME} ) ############# ## Testing ## ############# -catkin_add_gtest(test_${PROJECT_NAME} +find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(test_${PROJECT_NAME} test/testDiscreteTranscription.cpp test/testQpSolver.cpp test/testOcs2QpSolver.cpp ) + +target_include_directories(test_${PROJECT_NAME} + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/test/include +) + +ament_target_dependencies(test_${PROJECT_NAME} + ${dependencies} +) + target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} - gtest_main ) + +######################## +## Export Information ## +######################## + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${dependencies}) + +ament_package() diff --git a/ocs2_test_tools/ocs2_qp_solver/package.xml b/ocs2_test_tools/ocs2_qp_solver/package.xml index 440c5ce43..325797e67 100644 --- a/ocs2_test_tools/ocs2_qp_solver/package.xml +++ b/ocs2_test_tools/ocs2_qp_solver/package.xml @@ -1,5 +1,5 @@ - + ocs2_qp_solver 0.0.0 The ocs2_qp_solver package @@ -10,9 +10,21 @@ BSD - catkin + ament_cmake + eigen3_cmake_module + eigen3_cmake_module cmake_clang_tools + + eigen ocs2_core ocs2_oc + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + + From 1c8b5abade56b849bc1a7e44f16ef51966d92793 Mon Sep 17 00:00:00 2001 From: sunausti Date: Fri, 29 Aug 2025 00:27:39 +0000 Subject: [PATCH 30/42] ocs2_pinocchio_interface: porting to ros2 humble (#40) * ocs2_pinocchio_interface: porting to ros2 humble Signed-off-by: Austin Sun --- .../{ => ocs2_centroidal_model}/COLCON_IGNORE | 0 ocs2_pinocchio/ocs2_pinocchio/COLCON_IGNORE | 0 .../ocs2_pinocchio_interface/CMakeLists.txt | 122 +++++++++--------- .../cmake/pinocchio_config.cmake | 4 +- .../ocs2_pinocchio_interface/package.xml | 20 ++- .../ocs2_self_collision/COLCON_IGNORE | 0 .../COLCON_IGNORE | 0 .../ocs2_sphere_approximation/COLCON_IGNORE | 0 8 files changed, 82 insertions(+), 64 deletions(-) rename ocs2_pinocchio/{ => ocs2_centroidal_model}/COLCON_IGNORE (100%) create mode 100644 ocs2_pinocchio/ocs2_pinocchio/COLCON_IGNORE create mode 100644 ocs2_pinocchio/ocs2_self_collision/COLCON_IGNORE create mode 100644 ocs2_pinocchio/ocs2_self_collision_visualization/COLCON_IGNORE create mode 100644 ocs2_pinocchio/ocs2_sphere_approximation/COLCON_IGNORE diff --git a/ocs2_pinocchio/COLCON_IGNORE b/ocs2_pinocchio/ocs2_centroidal_model/COLCON_IGNORE similarity index 100% rename from ocs2_pinocchio/COLCON_IGNORE rename to ocs2_pinocchio/ocs2_centroidal_model/COLCON_IGNORE diff --git a/ocs2_pinocchio/ocs2_pinocchio/COLCON_IGNORE b/ocs2_pinocchio/ocs2_pinocchio/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/CMakeLists.txt b/ocs2_pinocchio/ocs2_pinocchio_interface/CMakeLists.txt index 0107f0be5..5042b4a1b 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/CMakeLists.txt @@ -1,71 +1,61 @@ -cmake_minimum_required(VERSION 3.0.2) -project(ocs2_pinocchio_interface) +cmake_minimum_required(VERSION 3.14.4) +project(ocs2_pinocchio_interface LANGUAGES CXX) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter -Wno-unused-variable -Wno-return-type -Wno-sign-compare -Wno-deprecated-declarations) +endif() # Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_robotic_tools -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) +## Find ament macros and libraries +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) find_package(PkgConfig REQUIRED) pkg_check_modules(pinocchio REQUIRED pinocchio) -find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(urdf REQUIRED) +find_package(urdfdom REQUIRED) +find_package(pinocchio REQUIRED) # Add pinocchio configurations include(cmake/pinocchio_config.cmake) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${pinocchio_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - pinocchio - CFG_EXTRAS - pinocchio_config.cmake -) - ########### ## Build ## ########### -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${pinocchio_INCLUDE_DIRS} -) - # ocs2 pinocchio interface library -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED src/PinocchioInterface.cpp src/PinocchioInterfaceCppAd.cpp src/PinocchioEndEffectorKinematics.cpp src/PinocchioEndEffectorKinematicsCppAd.cpp src/urdf.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} + +set(includes_${PROJECT_NAME} + $ + $ ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${pinocchio_LIBRARIES} + +target_include_directories(${PROJECT_NAME} PUBLIC + ${includes_${PROJECT_NAME}} ) -target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_PINOCCHIO_FLAGS}) + +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_PINOCCHIO_FLAGS} ${OCS2_CXX_FLAGS}) + +# Link ament dependencies +ament_target_dependencies(${PROJECT_NAME} ocs2_core ocs2_robotic_tools Boost Eigen3 pinocchio urdf urdfdom) #################### ## Clang tooling ### @@ -86,25 +76,41 @@ endif (cmake_clang_tools_FOUND) ## Install ## ############# -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# Install library +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + +# Install headers +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} ) +# Export targets +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(ocs2_core ocs2_robotic_tools pinocchio Eigen3) + ############ # Testing ## ############ -catkin_add_gtest(testPinocchioInterface - test/testPinocchioInterface.cpp - test/testPinocchioEndEffectorKinematics.cpp -) -target_link_libraries(testPinocchioInterface - gtest_main - ${PROJECT_NAME} - ${catkin_LIBRARIES} -) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + # Test pinocchio interface + ament_add_gtest(testPinocchioInterface + test/testPinocchioInterface.cpp + test/testPinocchioEndEffectorKinematics.cpp + ) + target_link_libraries(testPinocchioInterface + ${PROJECT_NAME} + ) + ament_target_dependencies(testPinocchioInterface ocs2_core ocs2_robotic_tools) +endif() + +ament_package(CONFIG_EXTRAS "cmake/pinocchio_config.cmake") diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/cmake/pinocchio_config.cmake b/ocs2_pinocchio/ocs2_pinocchio_interface/cmake/pinocchio_config.cmake index cca51d551..25246e4c3 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/cmake/pinocchio_config.cmake +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/cmake/pinocchio_config.cmake @@ -8,7 +8,7 @@ set(OCS2_PINOCCHIO_FLAGS ${OCS2_CXX_FLAGS} ${pinocchio_CFLAGS_OTHER} -Wno-ignored-attributes - -Wno-invalid-partial-specialization # to silence warning with unsupported Eigen Tensor +# -Wno-invalid-partial-specialization # to silence warning with unsupported Eigen Tensor -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR -) \ No newline at end of file +) diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/package.xml b/ocs2_pinocchio/ocs2_pinocchio_interface/package.xml index 25503afcb..9b6a67bc5 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/package.xml +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/package.xml @@ -1,5 +1,5 @@ - + ocs2_pinocchio_interface 0.0.0 The ocs2_pinocchio_interface package @@ -7,13 +7,25 @@ Michael Spieler Perry Franklin - TODO + BSD - catkin - cmake_clang_tools + ament_cmake + eigen3_cmake_module + eigen3_cmake_module + eigen ocs2_core ocs2_robotic_tools pinocchio + urdf + urdfdom + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + diff --git a/ocs2_pinocchio/ocs2_self_collision/COLCON_IGNORE b/ocs2_pinocchio/ocs2_self_collision/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_pinocchio/ocs2_self_collision_visualization/COLCON_IGNORE b/ocs2_pinocchio/ocs2_self_collision_visualization/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_pinocchio/ocs2_sphere_approximation/COLCON_IGNORE b/ocs2_pinocchio/ocs2_sphere_approximation/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb From 910b445874919fc9e4d8c832a6391f084b1aeb06 Mon Sep 17 00:00:00 2001 From: sunausti Date: Fri, 29 Aug 2025 08:31:48 +0000 Subject: [PATCH 31/42] ocs2_ddp: porting to ros2 humble (#32) * ocs2_ddp: porting to ros2 humble Signed-off-by: Austin Sun --- ocs2_ddp/CMakeLists.txt | 329 +++++++++++++++++--------------- ocs2_ddp/COLCON_IGNORE | 0 ocs2_ddp/package.xml | 19 +- ocs2_ddp/test/HybridSlqTest.cpp | 2 +- 4 files changed, 188 insertions(+), 162 deletions(-) delete mode 100644 ocs2_ddp/COLCON_IGNORE diff --git a/ocs2_ddp/CMakeLists.txt b/ocs2_ddp/CMakeLists.txt index 97606f278..06904af73 100644 --- a/ocs2_ddp/CMakeLists.txt +++ b/ocs2_ddp/CMakeLists.txt @@ -1,56 +1,32 @@ -cmake_minimum_required(VERSION 3.0.2) -project(ocs2_ddp) - -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ocs2_core - ocs2_oc - ocs2_mpc - ocs2_qp_solver -) -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - -find_package(Eigen3 3.3 REQUIRED NO_MODULE) +cmake_minimum_required(VERSION 3.14.4) +project(ocs2_ddp LANGUAGES CXX) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +## Find ament macros and libraries +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_oc REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_qp_solver REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) # Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ocs2_core - ocs2_oc - ocs2_mpc - ocs2_qp_solver - DEPENDS - Boost -) - ########### ## Build ## ########### -include_directories( - include - test/include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED src/riccati_equations/ContinuousTimeRiccatiEquations.cpp src/riccati_equations/DiscreteTimeRiccatiEquations.cpp src/riccati_equations/RiccatiModification.cpp @@ -65,15 +41,38 @@ add_library(${PROJECT_NAME} src/DDP_Settings.cpp src/DDP_HelperFunctions.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} -) + target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) +set(includes_${PROJECT_NAME} + $ + $ +) + +target_include_directories(${PROJECT_NAME} PUBLIC + ${includes_${PROJECT_NAME}} +) + +# Link ament dependencies +ament_target_dependencies(${PROJECT_NAME} ocs2_core ocs2_oc ocs2_mpc ocs2_qp_solver Boost Eigen3) + add_executable(${PROJECT_NAME}_lintTarget src/lintTarget.cpp ) +target_include_directories(${PROJECT_NAME}_lintTarget PUBLIC + ${includes_${PROJECT_NAME}} +) + +ament_target_dependencies(${PROJECT_NAME}_lintTarget + ocs2_core + ocs2_oc + ocs2_mpc + ocs2_qp_solver + Boost + Eigen3 +) + ######################### ### CLANG TOOLING ### ######################### @@ -92,122 +91,138 @@ endif(cmake_clang_tools_FOUND) ## Install ## ############# -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# Install library +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +# Install headers +install( + DIRECTORY include/ test/include/ + DESTINATION include/${PROJECT_NAME} +) + +# Export targets +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(ocs2_core ocs2_oc ocs2_mpc ocs2_qp_solver Boost Eigen3) ############# ## Testing ## ############# -## Info ============================== -## to run tests, cd package folder and run -## $ catkin build -DCMAKE_BUILD_TYPE=RelWithDebInfo --this -## $ catkin run_tests --no-deps --this -## to see the summary of unit test results run -## $ catkin_test_results ../../../build/ocs2_ddp - -catkin_add_gtest(exp0_ddp_test - test/Exp0Test.cpp -) -target_link_libraries(exp0_ddp_test - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - gtest_main -) -catkin_add_gtest(exp1_ddp_test - test/Exp1Test.cpp -) -target_link_libraries(exp1_ddp_test - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - gtest_main -) - -catkin_add_gtest(correctness_test - test/CorrectnessTest.cpp -) -target_link_libraries(correctness_test - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - gtest_main -) - -catkin_add_gtest(riccati_ode_test - test/RiccatiTest.cpp -) -target_link_libraries(riccati_ode_test - ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main -) - -catkin_add_gtest(circular_kinematics_ddp_test - test/CircularKinematicsTest.cpp -) -target_link_libraries(circular_kinematics_ddp_test - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - gtest_main -) - -catkin_add_gtest(hybrid_slq_test - test/HybridSlqTest.cpp -) -target_link_libraries(hybrid_slq_test - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - gtest_main -) - -catkin_add_gtest(bouncing_mass_test - test/bouncingmass/BouncingMassTest.cpp - test/bouncingmass/OverallReference.cpp - test/bouncingmass/Reference.cpp -) -target_link_libraries(bouncing_mass_test - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - gtest_main -) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + set(test_includes_${PROJECT_NAME} + ${includes_${PROJECT_NAME}} + $ + ) + + function(ament_add_gtest_wrapper test_name) + ament_add_gtest(${test_name} ${ARGN}) + target_include_directories(${test_name} PUBLIC + ${test_includes_${PROJECT_NAME}} + ) + endfunction() + + # Test exp0 + ament_add_gtest(exp0_ddp_test + test/Exp0Test.cpp + ) + target_link_libraries(exp0_ddp_test + ${PROJECT_NAME} + ) + ament_target_dependencies(exp0_ddp_test ocs2_core ocs2_oc ocs2_mpc ocs2_qp_solver Boost) + + # Test exp1 + ament_add_gtest_wrapper(exp1_ddp_test + test/Exp1Test.cpp + ) + target_link_libraries(exp1_ddp_test + ${PROJECT_NAME} + ) + ament_target_dependencies(exp1_ddp_test ocs2_core ocs2_oc ocs2_mpc ocs2_qp_solver Boost) -catkin_add_gtest(testContinuousTimeLqr - test/testContinuousTimeLqr.cpp -) -target_link_libraries(testContinuousTimeLqr - ${Boost_LIBRARIES} - ${catkin_LIBRARIES} - ${PROJECT_NAME} - gtest_main -) + # Correctness test + ament_add_gtest_wrapper(correctness_test + test/CorrectnessTest.cpp + TIMEOUT 120 + ) -catkin_add_gtest(testDdpHelperFunction - test/testDdpHelperFunction.cpp -) -target_link_libraries(testDdpHelperFunction - ${Boost_LIBRARIES} - ${catkin_LIBRARIES} - ${PROJECT_NAME} - gtest_main -) + target_link_libraries(correctness_test + ${PROJECT_NAME} + ) + ament_target_dependencies(correctness_test ocs2_core ocs2_oc ocs2_mpc ocs2_qp_solver Boost) + + # Riccati ODE test + ament_add_gtest_wrapper(riccati_ode_test + test/RiccatiTest.cpp + ) + target_link_libraries(riccati_ode_test + ${PROJECT_NAME} + ) + ament_target_dependencies(riccati_ode_test ocs2_core ocs2_oc ocs2_mpc ocs2_qp_solver) + + # Circular kinematics test + ament_add_gtest_wrapper(circular_kinematics_ddp_test + test/CircularKinematicsTest.cpp + TIMEOUT 120 + ) + target_link_libraries(circular_kinematics_ddp_test + ${PROJECT_NAME} + ) + ament_target_dependencies(circular_kinematics_ddp_test ocs2_core ocs2_oc ocs2_mpc ocs2_qp_solver Boost) + + # Hybrid SLQ test + ament_add_gtest_wrapper(hybrid_slq_test + test/HybridSlqTest.cpp + TIMEOUT 120 + ) + target_link_libraries(hybrid_slq_test + ${PROJECT_NAME} + ) + ament_target_dependencies(hybrid_slq_test ocs2_core ocs2_oc ocs2_mpc ocs2_qp_solver Boost) + + # Bouncing mass test + ament_add_gtest_wrapper(bouncing_mass_test + test/bouncingmass/BouncingMassTest.cpp + test/bouncingmass/OverallReference.cpp + test/bouncingmass/Reference.cpp + ) + target_link_libraries(bouncing_mass_test + ${PROJECT_NAME} + ) + ament_target_dependencies(bouncing_mass_test ocs2_core ocs2_oc ocs2_mpc ocs2_qp_solver Boost) + + # Continuous time LQR test + ament_add_gtest_wrapper(testContinuousTimeLqr + test/testContinuousTimeLqr.cpp + ) + target_link_libraries(testContinuousTimeLqr + ${PROJECT_NAME} + ) + ament_target_dependencies(testContinuousTimeLqr ocs2_core ocs2_oc ocs2_mpc ocs2_qp_solver Boost) + + # DDP helper function test + ament_add_gtest_wrapper(testDdpHelperFunction + test/testDdpHelperFunction.cpp + ) + target_link_libraries(testDdpHelperFunction + ${PROJECT_NAME} + ) + ament_target_dependencies(testDdpHelperFunction ocs2_core ocs2_oc ocs2_mpc ocs2_qp_solver Boost) + + # Reaching task test + ament_add_gtest_wrapper(testReachingTask + test/testReachingTask.cpp + ) + target_link_libraries(testReachingTask + ${PROJECT_NAME} + ) + ament_target_dependencies(testReachingTask ocs2_core ocs2_oc ocs2_mpc ocs2_qp_solver Boost) +endif() -catkin_add_gtest(testReachingTask - test/testReachingTask.cpp -) -target_link_libraries(testReachingTask - ${Boost_LIBRARIES} - ${catkin_LIBRARIES} - ${PROJECT_NAME} - gtest_main -) +ament_package() diff --git a/ocs2_ddp/COLCON_IGNORE b/ocs2_ddp/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/ocs2_ddp/package.xml b/ocs2_ddp/package.xml index c5f2723cd..f97938142 100644 --- a/ocs2_ddp/package.xml +++ b/ocs2_ddp/package.xml @@ -1,5 +1,5 @@ - + ocs2_ddp 0.0.0 The ocs2_ddp package @@ -10,12 +10,23 @@ BSD - catkin - cmake_modules - cmake_clang_tools + ament_cmake + eigen3_cmake_module + eigen3_cmake_module + + boost + eigen ocs2_qp_solver ocs2_core ocs2_oc ocs2_mpc + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/ocs2_ddp/test/HybridSlqTest.cpp b/ocs2_ddp/test/HybridSlqTest.cpp index 5930c7784..b564d1447 100644 --- a/ocs2_ddp/test/HybridSlqTest.cpp +++ b/ocs2_ddp/test/HybridSlqTest.cpp @@ -88,7 +88,7 @@ TEST(HybridSlqTest, state_rollout_slq) { settings.useFeedbackPolicy_ = true; settings.debugPrintRollout_ = false; settings.strategy_ = search_strategy::Type::LINE_SEARCH; - settings.lineSearch_.minStepLength = 1e-3; + settings.lineSearch_.minStepLength = 1e-4; return settings; }(); From c4c05422aa6dcfd221cea7c27041bba0f50b9954 Mon Sep 17 00:00:00 2001 From: xzhan34 Date: Mon, 1 Sep 2025 16:51:07 +0800 Subject: [PATCH 32/42] ocs2_ros_interfaces: porting for ros2 humble. (#37) rebased this repo to ros2 hubmle as the part of the ocs2 porting effort. Signed-off-by: Zhang, Xiaolin --- .../ReferenceManagerDecorator.h | 29 ++- ocs2_ros_interfaces/CMakeLists.txt | 135 +++++++------ ocs2_ros_interfaces/COLCON_IGNORE | 0 .../TargetTrajectoriesInteractiveMarker.h | 44 +++-- .../TargetTrajectoriesKeyboardPublisher.h | 47 +++-- .../command/TargetTrajectoriesRosPublisher.h | 24 ++- .../common/RosMsgConversions.h | 47 +++-- .../common/RosMsgHelpers.h | 29 +-- .../mpc/MPC_ROS_Interface.h | 78 ++++---- .../ocs2_ros_interfaces/mrt/DummyObserver.h | 7 +- .../mrt/LoopshapingDummyObserver.h | 16 +- .../mrt/MRT_ROS_Dummy_Loop.h | 49 +++-- .../mrt/MRT_ROS_Interface.h | 57 +++--- .../synchronized_module/RosReferenceManager.h | 64 ++++--- .../SolverObserverRosCallbacks.h | 78 +++++--- .../visualization/VisualizationHelpers.h | 100 ++++++---- ocs2_ros_interfaces/package.xml | 16 +- .../TargetTrajectoriesInteractiveMarker.cpp | 93 +++++---- .../TargetTrajectoriesKeyboardPublisher.cpp | 55 ++++-- .../TargetTrajectoriesRosPublisher.cpp | 18 +- .../src/common/RosMsgConversions.cpp | 155 +++++++++------ .../src/common/RosMsgHelpers.cpp | 26 +-- .../src/mpc/MPC_ROS_Interface.cpp | 180 +++++++++++------- .../src/mrt/LoopshapingDummyObserver.cpp | 19 +- .../src/mrt/MRT_ROS_Dummy_Loop.cpp | 86 ++++++--- .../src/mrt/MRT_ROS_Interface.cpp | 165 +++++++++------- .../src/multiplot/MultiplotRemap.cpp | 48 ++--- .../RosReferenceManager.cpp | 39 ++-- .../SolverObserverRosCallbacks.cpp | 145 +++++++++----- .../visualization/VisualizationHelpers.cpp | 56 +++--- .../test/test_custom_callback_queue.cpp | 53 ++---- 31 files changed, 1165 insertions(+), 793 deletions(-) delete mode 100644 ocs2_ros_interfaces/COLCON_IGNORE diff --git a/ocs2_oc/include/ocs2_oc/synchronized_module/ReferenceManagerDecorator.h b/ocs2_oc/include/ocs2_oc/synchronized_module/ReferenceManagerDecorator.h index ab8ee63c0..6adf6e2a2 100644 --- a/ocs2_oc/include/ocs2_oc/synchronized_module/ReferenceManagerDecorator.h +++ b/ocs2_oc/include/ocs2_oc/synchronized_module/ReferenceManagerDecorator.h @@ -28,6 +28,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ #pragma once +#include #include "ocs2_oc/synchronized_module/ReferenceManagerInterface.h" @@ -39,25 +40,37 @@ namespace ocs2 { */ class ReferenceManagerDecorator : public ReferenceManagerInterface { public: - explicit ReferenceManagerDecorator(std::shared_ptr referenceManagerPtr) + explicit ReferenceManagerDecorator( + std::shared_ptr referenceManagerPtr) : referenceManagerPtr_(std::move(referenceManagerPtr)) { if (referenceManagerPtr_ == nullptr) { - throw std::runtime_error("[ReferenceManagerDecorator] ReferenceManager pointer is nullptr!"); + throw std::runtime_error( + "[ReferenceManagerDecorator] ReferenceManager pointer is nullptr!"); } } ~ReferenceManagerDecorator() override = default; - void preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& initState) override { + void preSolverRun(scalar_t initTime, scalar_t finalTime, + const vector_t& initState) override { referenceManagerPtr_->preSolverRun(initTime, finalTime, initState); } - const ModeSchedule& getModeSchedule() const override { return referenceManagerPtr_->getModeSchedule(); } - void setModeSchedule(const ModeSchedule& modeSchedule) override { referenceManagerPtr_->setModeSchedule(modeSchedule); } - void setModeSchedule(ModeSchedule&& modeSchedule) override { referenceManagerPtr_->setModeSchedule(std::move(modeSchedule)); } + const ModeSchedule& getModeSchedule() const override { + return referenceManagerPtr_->getModeSchedule(); + } + void setModeSchedule(const ModeSchedule& modeSchedule) override { + referenceManagerPtr_->setModeSchedule(modeSchedule); + } + void setModeSchedule(ModeSchedule&& modeSchedule) override { + referenceManagerPtr_->setModeSchedule(std::move(modeSchedule)); + } - const TargetTrajectories& getTargetTrajectories() const override { return referenceManagerPtr_->getTargetTrajectories(); } - void setTargetTrajectories(const TargetTrajectories& targetTrajectories) override { + const TargetTrajectories& getTargetTrajectories() const override { + return referenceManagerPtr_->getTargetTrajectories(); + } + void setTargetTrajectories( + const TargetTrajectories& targetTrajectories) override { referenceManagerPtr_->setTargetTrajectories(targetTrajectories); } void setTargetTrajectories(TargetTrajectories&& targetTrajectories) override { diff --git a/ocs2_ros_interfaces/CMakeLists.txt b/ocs2_ros_interfaces/CMakeLists.txt index 0fb39c4c2..60b49ecd0 100644 --- a/ocs2_ros_interfaces/CMakeLists.txt +++ b/ocs2_ros_interfaces/CMakeLists.txt @@ -1,8 +1,32 @@ -cmake_minimum_required(VERSION 3.0.2) -project(ocs2_ros_interfaces) +cmake_minimum_required(VERSION 3.14.4) +project(ocs2_ros_interfaces LANGUAGES CXX) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wno-extra -Wpedantic -Wno-reorder) +endif() + +## Find ament_cmake macros and libraries +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(ocs2_msgs REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(std_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(interactive_markers REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3) -set(CATKIN_PACKAGE_DEPENDENCIES - roscpp +########### +## Build ## +########### + +set(dependencies + rclcpp ocs2_msgs ocs2_core ocs2_mpc @@ -10,49 +34,11 @@ set(CATKIN_PACKAGE_DEPENDENCIES visualization_msgs geometry_msgs interactive_markers + Boost + Eigen3 ) -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - cmake_modules - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -find_package(Boost REQUIRED COMPONENTS filesystem) - -# Generate compile_commands.json for clang tools -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -################################### -## catkin specific configuration ## -################################### -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost -) - -########### -## Build ## -########### - -## Specify additional locations of header files -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED src/command/TargetTrajectoriesRosPublisher.cpp src/command/TargetTrajectoriesInteractiveMarker.cpp src/command/TargetTrajectoriesKeyboardPublisher.cpp @@ -67,19 +53,26 @@ add_library(${PROJECT_NAME} src/visualization/VisualizationHelpers.cpp src/visualization/VisualizationColors.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +set(includes_${PROJECT_NAME} + $ + $ +) +target_include_directories(${PROJECT_NAME} PUBLIC + ${includes_${PROJECT_NAME}} +) +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) add_executable(test_custom_callback_queue test/test_custom_callback_queue.cpp ) -add_dependencies(test_custom_callback_queue - ${catkin_EXPORTED_TARGETS} +ament_target_dependencies(test_custom_callback_queue + ${dependencies} ) target_link_libraries(test_custom_callback_queue - ${catkin_LIBRARIES} + ${PROJECT_NAME} ) target_compile_options(test_custom_callback_queue PRIVATE ${OCS2_CXX_FLAGS}) @@ -87,23 +80,22 @@ target_compile_options(test_custom_callback_queue PRIVATE ${OCS2_CXX_FLAGS}) add_executable(multiplot_remap src/multiplot/MultiplotRemap.cpp ) -add_dependencies(multiplot_remap - ${catkin_EXPORTED_TARGETS} +ament_target_dependencies(multiplot_remap + ${dependencies} ) -target_link_libraries(multiplot_remap +target_link_libraries(test_custom_callback_queue ${PROJECT_NAME} - ${catkin_LIBRARIES} ) target_compile_options(multiplot_remap PRIVATE ${OCS2_CXX_FLAGS}) add_executable(${PROJECT_NAME}_lintTarget src/lintTarget.cpp ) -add_dependencies(${PROJECT_NAME}_lintTarget - ${catkin_EXPORTED_TARGETS} +target_include_directories(${PROJECT_NAME}_lintTarget PUBLIC + ${includes_${PROJECT_NAME}} ) -target_link_libraries(${PROJECT_NAME}_lintTarget - ${catkin_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME}_lintTarget + ${dependencies} ) ######################### @@ -124,21 +116,22 @@ endif(cmake_clang_tools_FOUND) ## Install ## ############# -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) -install(TARGETS multiplot_remap - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) -install(DIRECTORY launch multiplot - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(TARGETS multiplot_remap DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY multiplot + DESTINATION include/${PROJECT_NAME}/ ) +ament_export_dependencies(${dependencies}) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ############# ## Testing ## @@ -149,3 +142,5 @@ install(DIRECTORY launch multiplot ## $ catkin run_tests --no-deps --this ## to see the summary of unit test results run ## $ catkin_test_results ../../../build/ocs2_ros_interfaces + +ament_package() diff --git a/ocs2_ros_interfaces/COLCON_IGNORE b/ocs2_ros_interfaces/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesInteractiveMarker.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesInteractiveMarker.h index db286a144..29ae60cba 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesInteractiveMarker.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesInteractiveMarker.h @@ -29,16 +29,15 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include +#include + #include +#include +#include #include #include -#include -#include - -#include -#include - namespace ocs2 { /** @@ -47,36 +46,45 @@ namespace ocs2 { class TargetTrajectoriesInteractiveMarker final { public: using GaolPoseToTargetTrajectories = std::function; + const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation, + const SystemObservation& observation)>; /** * Constructor * - * @param [in] nodeHandle: ROS node handle. - * @param [in] topicPrefix: The TargetTrajectories will be published on "topicPrefix_mpc_target" topic. Moreover, the latest - * observation is be expected on "topicPrefix_mpc_observation" topic. - * @param [in] gaolPoseToTargetTrajectories: A function which transforms the commanded pose to TargetTrajectories. + * @param [in] node: ROS node handle. + * @param [in] topicPrefix: The TargetTrajectories will be published on + * "topicPrefix_mpc_target" topic. Moreover, the latest observation is be + * expected on "topicPrefix_mpc_observation" topic. + * @param [in] gaolPoseToTargetTrajectories: A function which transforms the + * commanded pose to TargetTrajectories. */ - TargetTrajectoriesInteractiveMarker(::ros::NodeHandle& nodeHandle, const std::string& topicPrefix, - GaolPoseToTargetTrajectories gaolPoseToTargetTrajectories); + TargetTrajectoriesInteractiveMarker( + const rclcpp::Node::SharedPtr& node, const std::string& topicPrefix, + GaolPoseToTargetTrajectories gaolPoseToTargetTrajectories); /** * Spins ROS to update the interactive markers. */ - void publishInteractiveMarker() { ::ros::spin(); } + void publishInteractiveMarker() { rclcpp::spin(node_); } private: - visualization_msgs::InteractiveMarker createInteractiveMarker() const; - void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); + visualization_msgs::msg::InteractiveMarker createInteractiveMarker() const; + void processFeedback( + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& + feedback); + rclcpp::Node::SharedPtr node_; interactive_markers::MenuHandler menuHandler_; interactive_markers::InteractiveMarkerServer server_; GaolPoseToTargetTrajectories gaolPoseToTargetTrajectories_; - std::unique_ptr targetTrajectoriesPublisherPtr_; + std::unique_ptr + targetTrajectoriesPublisherPtr_; - ::ros::Subscriber observationSubscriber_; + rclcpp::Subscription::SharedPtr + observationSubscriber_; mutable std::mutex latestObservationMutex_; SystemObservation latestObservation_; }; diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesKeyboardPublisher.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesKeyboardPublisher.h index 731975c06..f73af2ff1 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesKeyboardPublisher.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesKeyboardPublisher.h @@ -29,14 +29,14 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include +#include + #include #include #include -#include - -#include -#include +#include namespace ocs2 { @@ -45,43 +45,52 @@ namespace ocs2 { */ class TargetTrajectoriesKeyboardPublisher final { public: - using CommandLineToTargetTrajectories = - std::function; + using CommandLineToTargetTrajectories = std::function; /** * Constructor * - * @param [in] nodeHandle: ROS node handle. - * @param [in] topicPrefix: The TargetTrajectories will be published on "topicPrefix_mpc_target" topic. Moreover, the latest - * observation is be expected on "topicPrefix_mpc_observation" topic. - * @param [in] targetCommandLimits: The limits of the loaded command from command-line (for safety purposes). - * @param [in] commandLineToTargetTrajectoriesFun: A function which transforms the command line input to TargetTrajectories. + * @param [in] node: ROS node handle. + * @param [in] topicPrefix: The TargetTrajectories will be published on + * "topicPrefix_mpc_target" topic. Moreover, the latest observation is be + * expected on "topicPrefix_mpc_observation" topic. + * @param [in] targetCommandLimits: The limits of the loaded command from + * command-line (for safety purposes). + * @param [in] commandLineToTargetTrajectoriesFun: A function which transforms + * the command line input to TargetTrajectories. */ - TargetTrajectoriesKeyboardPublisher(::ros::NodeHandle& nodeHandle, const std::string& topicPrefix, - const scalar_array_t& targetCommandLimits, - CommandLineToTargetTrajectories commandLineToTargetTrajectoriesFun); + TargetTrajectoriesKeyboardPublisher( + const rclcpp::Node::SharedPtr& node, const std::string& topicPrefix, + const scalar_array_t& targetCommandLimits, + CommandLineToTargetTrajectories commandLineToTargetTrajectoriesFun); /** Gets the command vector size. */ size_t targetCommandSize() const { return targetCommandLimits_.size(); } /** - * Publishes command line input. If the input command is shorter than the expected command - * size (targetCommandSize), the method will set the rest of the command to zero. + * Publishes command line input. If the input command is shorter than the + * expected command size (targetCommandSize), the method will set the rest of + * the command to zero. * * @param [in] commadMsg: Message to be displayed on screen. */ - void publishKeyboardCommand(const std::string& commadMsg = "Enter command, separated by space"); + void publishKeyboardCommand( + const std::string& commadMsg = "Enter command, separated by space"); private: /** Gets the target from command line. */ vector_t getCommandLine(); + rclcpp::Node::SharedPtr node_; const vector_t targetCommandLimits_; CommandLineToTargetTrajectories commandLineToTargetTrajectoriesFun_; - std::unique_ptr targetTrajectoriesPublisherPtr_; + std::unique_ptr + targetTrajectoriesPublisherPtr_; - ::ros::Subscriber observationSubscriber_; + rclcpp::Subscription::SharedPtr + observationSubscriber_; mutable std::mutex latestObservationMutex_; SystemObservation latestObservation_; }; diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesRosPublisher.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesRosPublisher.h index ffa79f70b..ed895c65b 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesRosPublisher.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/command/TargetTrajectoriesRosPublisher.h @@ -29,29 +29,31 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include -#include - -#include - #include #include +#include +#include +#include + #include "ocs2_ros_interfaces/common/RosMsgConversions.h" +#include namespace ocs2 { /** * This class provides a ROS publisher for the TargetTrajectories. */ -class TargetTrajectoriesRosPublisher final { +class TargetTrajectoriesRosPublisher { public: /** * Constructor. - * @param [in] nodeHandle: ROS node handle. - * @param [in] topicPrefix: The TargetTrajectories will be published on "topicPrefix_mpc_target" topic. + * @param [in] topicPrefix: The TargetTrajectories will be published on + * "topicPrefix_mpc_target" topic. */ - TargetTrajectoriesRosPublisher(::ros::NodeHandle& nodeHandle, const std::string& topicPrefix = "anonymousRobot"); + TargetTrajectoriesRosPublisher( + rclcpp::Node::SharedPtr node, + const std::string& topicPrefix = "anonymousRobot"); /** Destructor. */ ~TargetTrajectoriesRosPublisher(); @@ -60,7 +62,9 @@ class TargetTrajectoriesRosPublisher final { void publishTargetTrajectories(const TargetTrajectories& targetTrajectories); private: - ::ros::Publisher targetTrajectoriesPublisher_; + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr + targetTrajectoriesPublisher_; }; } // namespace ocs2 diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h index e390d0241..1c59c8835 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h @@ -38,34 +38,40 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include // MPC messages -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include namespace ocs2 { namespace ros_msg_conversions { /** Creates the observation message. */ -ocs2_msgs::mpc_observation createObservationMsg(const SystemObservation& observation); +ocs2_msgs::msg::MpcObservation createObservationMsg( + const SystemObservation& observation); /** Reads the observation message. */ -SystemObservation readObservationMsg(const ocs2_msgs::mpc_observation& observationMsg); +SystemObservation readObservationMsg( + const ocs2_msgs::msg::MpcObservation& observationMsg); /** Creates the mode sequence message. */ -ocs2_msgs::mode_schedule createModeScheduleMsg(const ModeSchedule& modeSchedule); +ocs2_msgs::msg::ModeSchedule createModeScheduleMsg( + const ModeSchedule& modeSchedule); /** Reads the mode sequence message. */ -ModeSchedule readModeScheduleMsg(const ocs2_msgs::mode_schedule& modeScheduleMsg); +ModeSchedule readModeScheduleMsg( + const ocs2_msgs::msg::ModeSchedule& modeScheduleMsg); /** Creates the target trajectories message. */ -ocs2_msgs::mpc_target_trajectories createTargetTrajectoriesMsg(const TargetTrajectories& targetTrajectories); +ocs2_msgs::msg::MpcTargetTrajectories createTargetTrajectoriesMsg( + const TargetTrajectories& targetTrajectories); /** Returns the TargetTrajectories message. */ -TargetTrajectories readTargetTrajectoriesMsg(const ocs2_msgs::mpc_target_trajectories& targetTrajectoriesMsg); +TargetTrajectories readTargetTrajectoriesMsg( + const ocs2_msgs::msg::MpcTargetTrajectories& targetTrajectoriesMsg); /** * Creates the performance indices message. @@ -74,19 +80,24 @@ TargetTrajectories readTargetTrajectoriesMsg(const ocs2_msgs::mpc_target_traject * @param [in] performanceIndices: The performance indices of the solver. * @return The performance indices ROS message. */ -ocs2_msgs::mpc_performance_indices createPerformanceIndicesMsg(scalar_t initTime, const PerformanceIndex& performanceIndices); +ocs2_msgs::msg::MpcPerformanceIndices createPerformanceIndicesMsg( + scalar_t initTime, const PerformanceIndex& performanceIndices); /** Reads the performance indices message. */ -PerformanceIndex readPerformanceIndicesMsg(const ocs2_msgs::mpc_performance_indices& performanceIndicesMsg); +PerformanceIndex readPerformanceIndicesMsg( + const ocs2_msgs::msg::MpcPerformanceIndices& performanceIndicesMsg); /** Creates constraint message. */ -ocs2_msgs::constraint createConstraintMsg(scalar_t time, const vector_t& constraint); +ocs2_msgs::msg::Constraint createConstraintMsg(scalar_t time, + const vector_t& constraint); /** Creates lagrangian_metrics message. */ -ocs2_msgs::lagrangian_metrics createLagrangianMetricsMsg(scalar_t time, LagrangianMetricsConstRef metrics); +ocs2_msgs::msg::LagrangianMetrics createLagrangianMetricsMsg( + scalar_t time, LagrangianMetricsConstRef metrics); /** Creates multiplier message. */ -ocs2_msgs::multiplier createMultiplierMsg(scalar_t time, MultiplierConstRef multiplier); +ocs2_msgs::msg::Multiplier createMultiplierMsg(scalar_t time, + MultiplierConstRef multiplier); } // namespace ros_msg_conversions } // namespace ocs2 diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgHelpers.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgHelpers.h index 8bdd89e2a..06bdad2c7 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgHelpers.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgHelpers.h @@ -29,30 +29,35 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include - #include #include +#include +#include +#include +#include +#include -#include -#include -#include -#include +#include namespace ocs2 { namespace ros_msg_helpers { -geometry_msgs::Vector3 getVectorMsg(const Eigen::Vector3d& vec); +geometry_msgs::msg::Vector3 getVectorMsg(const Eigen::Vector3d& vec); -geometry_msgs::Point getPointMsg(const Eigen::Vector3d& point); +geometry_msgs::msg::Point getPointMsg(const Eigen::Vector3d& point); -geometry_msgs::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientation); +geometry_msgs::msg::Quaternion getOrientationMsg( + const Eigen::Quaterniond& orientation); -std_msgs::Header getHeaderMsg(const std::string& frame_id, const ros::Time& timeStamp); +std_msgs::msg::Header getHeaderMsg(const std::string& frame_id, + const rclcpp::Time& timeStamp); -visualization_msgs::Marker getLineMsg(std::vector&& points, std::array color, double lineWidth); +visualization_msgs::msg::Marker getLineMsg( + std::vector&& points, + std::array color, double lineWidth); -std_msgs::ColorRGBA getColor(std::array rgb, double alpha = 1.0); +std_msgs::msg::ColorRGBA getColor(std::array rgb, + double alpha = 1.0); } // namespace ros_msg_helpers } // namespace ocs2 diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h index fe0611e6e..2de03e08e 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h @@ -29,32 +29,29 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include +#include +#include +#include +#include +#include +#include + #include #include #include #include #include +#include +#include +#include +#include +#include #include #include #include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include +#include #define PUBLISH_THREAD @@ -71,7 +68,8 @@ class MPC_ROS_Interface { * @param [in] mpc: The underlying MPC class to be used. * @param [in] topicPrefix: The robot's name. */ - explicit MPC_ROS_Interface(MPC_BASE& mpc, std::string topicPrefix = "anonymousRobot"); + explicit MPC_ROS_Interface(MPC_BASE& mpc, + std::string topicPrefix = "anonymousRobot"); /** * Destructor. @@ -96,11 +94,12 @@ class MPC_ROS_Interface { void spin(); /** - * This is the main routine which launches all the nodes required for MPC to run which includes: - * (1) The MPC policy publisher (either feedback or feedforward policy). - * (2) The observation subscriber which gets the current measured state to invoke the MPC run routine. + * This is the main routine which launches all the nodes required for MPC to + * run which includes: (1) The MPC policy publisher (either feedback or + * feedforward policy). (2) The observation subscriber which gets the current + * measured state to invoke the MPC run routine. */ - void launchNodes(ros::NodeHandle& nodeHandle); + void launchNodes(const rclcpp::Node::SharedPtr& node); protected: /** @@ -109,7 +108,9 @@ class MPC_ROS_Interface { * @param req: Service request. * @param res: Service response. */ - bool resetMpcCallback(ocs2_msgs::reset::Request& req, ocs2_msgs::reset::Response& res); + bool resetMpcCallback( + const std::shared_ptr req, + std::shared_ptr res); /** * Creates MPC Policy message. @@ -119,8 +120,9 @@ class MPC_ROS_Interface { * @param [in] performanceIndices: The performance indices data of the solver. * @return MPC policy message. */ - static ocs2_msgs::mpc_flattened_controller createMpcPolicyMsg(const PrimalSolution& primalSolution, const CommandData& commandData, - const PerformanceIndex& performanceIndices); + static ocs2_msgs::msg::MpcFlattenedController createMpcPolicyMsg( + const PrimalSolution& primalSolution, const CommandData& commandData, + const PerformanceIndex& performanceIndices); /** * Handles ROS publishing thread. @@ -128,19 +130,21 @@ class MPC_ROS_Interface { void publisherWorker(); /** - * Updates the buffer variables from the MPC object. This method is automatically called by advanceMpc() + * Updates the buffer variables from the MPC object. This method is + * automatically called by advanceMpc() * * @param [in] mpcInitObservation: The observation used to run the MPC. */ void copyToBuffer(const SystemObservation& mpcInitObservation); /** - * The callback method which receives the current observation, invokes the MPC algorithm, - * and finally publishes the optimized policy. + * The callback method which receives the current observation, invokes the MPC + * algorithm, and finally publishes the optimized policy. * * @param [in] msg: The observation message. */ - void mpcObservationCallback(const ocs2_msgs::mpc_observation::ConstPtr& msg); + void mpcObservationCallback( + const ocs2_msgs::msg::MpcObservation::ConstSharedPtr& msg); protected: /* @@ -150,13 +154,16 @@ class MPC_ROS_Interface { std::string topicPrefix_; - std::shared_ptr nodeHandlerPtr_; + rclcpp::Node::SharedPtr node_; // Publishers and subscribers - ::ros::Subscriber mpcObservationSubscriber_; - ::ros::Subscriber mpcTargetTrajectoriesSubscriber_; - ::ros::Publisher mpcPolicyPublisher_; - ::ros::ServiceServer mpcResetServiceServer_; + rclcpp::Subscription::SharedPtr + mpcObservationSubscriber_; + rclcpp::Subscription::SharedPtr + mpcTargetTrajectoriesSubscriber_; + rclcpp::Publisher::SharedPtr + mpcPolicyPublisher_; + rclcpp::Service::SharedPtr mpcResetServiceServer_; std::unique_ptr bufferCommandPtr_; std::unique_ptr publisherCommandPtr_; @@ -165,7 +172,8 @@ class MPC_ROS_Interface { std::unique_ptr bufferPerformanceIndicesPtr_; std::unique_ptr publisherPerformanceIndicesPtr_; - mutable std::mutex bufferMutex_; // for policy variables with prefix (buffer*) + mutable std::mutex + bufferMutex_; // for policy variables with prefix (buffer*) // multi-threading for publishers std::atomic_bool terminateThread_{false}; diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/DummyObserver.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/DummyObserver.h index f1270bf40..c8f34f32f 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/DummyObserver.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/DummyObserver.h @@ -36,7 +36,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { /** - * This class can be used to observe the dummy loop. Every loop of the dummy, the update method is called on all subscribed observers. + * This class can be used to observe the dummy loop. Every loop of the dummy, + * the update method is called on all subscribed observers. */ class DummyObserver { public: @@ -49,6 +50,8 @@ class DummyObserver { * @param primalSolution : latest MPC primal solution * @param command : latest command on which the MPC solution is based */ - virtual void update(const SystemObservation& observation, const PrimalSolution& primalSolution, const CommandData& command) = 0; + virtual void update(const SystemObservation& observation, + const PrimalSolution& primalSolution, + const CommandData& command) = 0; }; } // namespace ocs2 diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/LoopshapingDummyObserver.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/LoopshapingDummyObserver.h index fbe132156..3d5add435 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/LoopshapingDummyObserver.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/LoopshapingDummyObserver.h @@ -35,18 +35,24 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { /** - * This wraps dummy observers and applies the loopshaping conversion before forwarding the update call. + * This wraps dummy observers and applies the loopshaping conversion before + * forwarding the update call. */ class LoopshapingDummyObserver : public DummyObserver { public: - LoopshapingDummyObserver(std::shared_ptr loopshapingDefinitionPtr, - std::vector> observersPtrArray); + LoopshapingDummyObserver( + std::shared_ptr loopshapingDefinitionPtr, + std::vector> observersPtrArray); ~LoopshapingDummyObserver() override = default; - void update(const SystemObservation& observation, const PrimalSolution& primalSolution, const CommandData& command) override; + void update(const SystemObservation& observation, + const PrimalSolution& primalSolution, + const CommandData& command) override; - void add(std::shared_ptr observer) { observersPtrArray_.push_back(std::move(observer)); } + void add(std::shared_ptr observer) { + observersPtrArray_.push_back(std::move(observer)); + } private: std::shared_ptr loopshapingDefinitionPtr_; diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h index 1b7f56b45..5eaec42e8 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h @@ -36,20 +36,25 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { /** - * This class implements a loop to test MPC-MRT communication interface using ROS. + * This class implements a loop to test MPC-MRT communication interface using + * ROS. */ class MRT_ROS_Dummy_Loop { public: /** * Constructor. * - * @param [in] mrt: The underlying MRT class to be used. If MRT contains a rollout object, the dummy will roll out - * the received controller using the MRT::rolloutPolicy() method instead of just sending back a planned state. - * @param [in] mrtDesiredFrequency: MRT loop frequency in Hz. This should always set to a positive number. - * @param [in] mpcDesiredFrequency: MPC loop frequency in Hz. If set to a positive number, MPC loop - * will be simulated to run by this frequency. Note that this might not be the MPC's real-time frequency. + * @param [in] mrt: The underlying MRT class to be used. If MRT contains a + * rollout object, the dummy will roll out the received controller using the + * MRT::rolloutPolicy() method instead of just sending back a planned state. + * @param [in] mrtDesiredFrequency: MRT loop frequency in Hz. This should + * always set to a positive number. + * @param [in] mpcDesiredFrequency: MPC loop frequency in Hz. If set to a + * positive number, MPC loop will be simulated to run by this frequency. Note + * that this might not be the MPC's real-time frequency. */ - MRT_ROS_Dummy_Loop(MRT_ROS_Interface& mrt, scalar_t mrtDesiredFrequency, scalar_t mpcDesiredFrequency = -1); + MRT_ROS_Dummy_Loop(MRT_ROS_Interface& mrt, scalar_t mrtDesiredFrequency, + scalar_t mpcDesiredFrequency = -1); /** * Destructor. @@ -62,15 +67,20 @@ class MRT_ROS_Dummy_Loop { * @param [in] initObservation: The initial observation. * @param [in] initTargetTrajectories: The initial TargetTrajectories. */ - void run(const SystemObservation& initObservation, const TargetTrajectories& initTargetTrajectories); + void run(const SystemObservation& initObservation, + const TargetTrajectories& initTargetTrajectories); /** - * Subscribe a set of observers to the dummy loop. Observers are updated in the provided order at the end of each timestep. - * The previous list of observers is overwritten. + * Subscribe a set of observers to the dummy loop. Observers are updated in + * the provided order at the end of each timestep. The previous list of + * observers is overwritten. * * @param observers : vector of observers. */ - void subscribeObservers(const std::vector>& observers) { observers_ = observers; } + void subscribeObservers( + const std::vector>& observers) { + observers_ = observers; + } protected: /** @@ -82,18 +92,23 @@ class MRT_ROS_Dummy_Loop { private: /** - * Runs a loop where mpc optimizations are synchronized with the forward simulation of the system + * Runs a loop where mpc optimizations are synchronized with the forward + * simulation of the system */ - void synchronizedDummyLoop(const SystemObservation& initObservation, const TargetTrajectories& initTargetTrajectories); + void synchronizedDummyLoop(const SystemObservation& initObservation, + const TargetTrajectories& initTargetTrajectories); /** - * Runs a loop where mpc optimizations and simulation of the system are asynchronous. - * The simulation runs as the specified mrtFrequency, and the MPC runs as fast as possible. + * Runs a loop where mpc optimizations and simulation of the system are + * asynchronous. The simulation runs as the specified mrtFrequency, and the + * MPC runs as fast as possible. */ - void realtimeDummyLoop(const SystemObservation& initObservation, const TargetTrajectories& initTargetTrajectories); + void realtimeDummyLoop(const SystemObservation& initObservation, + const TargetTrajectories& initTargetTrajectories); /** Forward simulates the system from current observation*/ - SystemObservation forwardSimulation(const SystemObservation& currentObservation); + SystemObservation forwardSimulation( + const SystemObservation& currentObservation); MRT_ROS_Interface& mrt_; std::vector> observers_; diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Interface.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Interface.h index 797ba6f2f..b1d04cf5e 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Interface.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mrt/MRT_ROS_Interface.h @@ -37,16 +37,14 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include -#include -#include +#include // MPC messages -#include -#include - #include +#include +#include + #include "ocs2_ros_interfaces/common/RosMsgConversions.h" #define PUBLISH_THREAD @@ -54,19 +52,20 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { /** - * This class implements MRT (Model Reference Tracking) communication interface using ROS. + * This class implements MRT (Model Reference Tracking) communication interface + * using ROS. */ class MRT_ROS_Interface : public MRT_BASE { public: /** * Constructor * - * @param [in] topicPrefix: The prefix defines the names for: observation's publishing topic "topicPrefix_mpc_observation", - * policy's receiving topic "topicPrefix_mpc_policy", and MPC reset service "topicPrefix_mpc_reset". + * @param [in] topicPrefix: The prefix defines the names for: observation's + * publishing topic "topicPrefix_mpc_observation", policy's receiving topic + * "topicPrefix_mpc_policy", and MPC reset service "topicPrefix_mpc_reset". * @param [in] mrtTransportHints: ROS transmission protocol. */ - explicit MRT_ROS_Interface(std::string topicPrefix = "anonymousRobot", - ::ros::TransportHints mrtTransportHints = ::ros::TransportHints().tcpNoDelay()); + explicit MRT_ROS_Interface(std::string topicPrefix = "anonymousRobot"); /** * Destructor @@ -91,12 +90,14 @@ class MRT_ROS_Interface : public MRT_BASE { void spinMRT(); /** - * Launches the ROS publishers and subscribers to communicate with the MPC node. - * @param nodeHandle + * Launches the ROS publishers and subscribers to communicate with the MPC + * node. + * @param node */ - void launchNodes(::ros::NodeHandle& nodeHandle); + void launchNodes(const rclcpp::Node::SharedPtr& node); - void setCurrentObservation(const SystemObservation& currentObservation) override; + void setCurrentObservation( + const SystemObservation& currentObservation) override; private: /** @@ -105,7 +106,8 @@ class MRT_ROS_Interface : public MRT_BASE { * * @param [in] msg: A constant pointer to the message */ - void mpcPolicyCallback(const ocs2_msgs::mpc_flattened_controller::ConstPtr& msg); + void mpcPolicyCallback( + const ocs2_msgs::msg::MpcFlattenedController::ConstSharedPtr& msg); /** * Helper function to read a MPC policy message. @@ -115,11 +117,14 @@ class MRT_ROS_Interface : public MRT_BASE { * @param [out] primalSolution: The MPC policy data * @param [out] performanceIndices: The MPC performance indices data */ - static void readPolicyMsg(const ocs2_msgs::mpc_flattened_controller& msg, CommandData& commandData, PrimalSolution& primalSolution, + static void readPolicyMsg(const ocs2_msgs::msg::MpcFlattenedController& msg, + CommandData& commandData, + PrimalSolution& primalSolution, PerformanceIndex& performanceIndices); /** - * A thread function which sends the current state and checks for a new MPC update. + * A thread function which sends the current state and checks for a new MPC + * update. */ void publisherWorkerThread(); @@ -127,16 +132,18 @@ class MRT_ROS_Interface : public MRT_BASE { std::string topicPrefix_; // Publishers and subscribers - ::ros::Publisher mpcObservationPublisher_; - ::ros::Subscriber mpcPolicySubscriber_; - ::ros::ServiceClient mpcResetServiceClient_; + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr + mpcObservationPublisher_; + rclcpp::Subscription::SharedPtr + mpcPolicySubscriber_; + rclcpp::Client::SharedPtr mpcResetServiceClient_; // ROS messages - ocs2_msgs::mpc_observation mpcObservationMsg_; - ocs2_msgs::mpc_observation mpcObservationMsgBuffer_; + ocs2_msgs::msg::MpcObservation mpcObservationMsg_; + ocs2_msgs::msg::MpcObservation mpcObservationMsgBuffer_; - ::ros::CallbackQueue mrtCallbackQueue_; - ::ros::TransportHints mrtTransportHints_; + // rclcpp::executors::SingleThreadedExecutor callback_executor_; // Multi-threading for publishers bool terminateThread_; diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h index cd3611392..0dbfbc625 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h @@ -29,64 +29,80 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include + #include +#include +#include #include #include -#include - -#include +#include namespace ocs2 { /** - * Decorates ReferenceManager with ROS subscribers to receive ModeSchedule and TargetTrajectories through ROS messages. + * Decorates ReferenceManager with ROS subscribers to receive ModeSchedule and + * TargetTrajectories through ROS messages. */ class RosReferenceManager final : public ReferenceManagerDecorator { public: /** * Constructor which decorates referenceManagerPtr. * - * @param [in] topicPrefix: The ReferenceManager will subscribe to "topicPrefix_mode_schedule" and "topicPrefix_mpc_target" - * @param [in] referenceManagerPtr: The ReferenceManager which will be decorated with ROS subscribers functionalities. - * topics to receive user-commanded ModeSchedule and TargetTrajectories respectively. + * @param [in] topicPrefix: The ReferenceManager will subscribe to + * "topicPrefix_mode_schedule" and "topicPrefix_mpc_target" + * @param [in] referenceManagerPtr: The ReferenceManager which will be + * decorated with ROS subscribers functionalities. topics to receive + * user-commanded ModeSchedule and TargetTrajectories respectively. */ - explicit RosReferenceManager(std::string topicPrefix, std::shared_ptr referenceManagerPtr); + explicit RosReferenceManager( + std::string topicPrefix, + std::shared_ptr referenceManagerPtr); ~RosReferenceManager() override = default; /** - * Creates a pointer to RosReferenceManager using a the derived class of type ReferenceManagerInterface, i.e. - * DerivedReferenceManager(args...). + * Creates a pointer to RosReferenceManager using a the derived class of type + * ReferenceManagerInterface, i.e. DerivedReferenceManager(args...). * - * @param [in] topicPrefix: The ReferenceManager will subscribe to "topicPrefix_mode_schedule" and "topicPrefix_mpc_target" - * topics to receive user-commanded ModeSchedule and TargetTrajectories respectively. - * @param args: arguments to forward to the constructor of DerivedReferenceManager + * @param [in] topicPrefix: The ReferenceManager will subscribe to + * "topicPrefix_mode_schedule" and "topicPrefix_mpc_target" topics to receive + * user-commanded ModeSchedule and TargetTrajectories respectively. + * @param args: arguments to forward to the constructor of + * DerivedReferenceManager */ template - static std::unique_ptr create(const std::string& topicPrefix, Args&&... args); + static std::unique_ptr create( + const std::string& topicPrefix, Args&&... args); /** - * Subscribers to "topicPrefix_mode_schedule" and "topicPrefix_mpc_target" topics to receive respectively: - * (1) ModeSchedule : The predefined mode schedule for time-triggered hybrid systems. - * (2) TargetTrajectories : The commanded TargetTrajectories. + * Subscribers to "topicPrefix_mode_schedule" and "topicPrefix_mpc_target" + * topics to receive respectively: (1) ModeSchedule : The predefined mode + * schedule for time-triggered hybrid systems. (2) TargetTrajectories : The + * commanded TargetTrajectories. */ - void subscribe(ros::NodeHandle& nodeHandle); + void subscribe(const rclcpp::Node::SharedPtr& node); private: const std::string topicPrefix_; - - ::ros::Subscriber modeScheduleSubscriber_; - ::ros::Subscriber targetTrajectoriesSubscriber_; + rclcpp::Node::SharedPtr node_; + rclcpp::Subscription::SharedPtr + modeScheduleSubscriber_; + rclcpp::Subscription::SharedPtr + targetTrajectoriesSubscriber_; }; /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ template -std::unique_ptr RosReferenceManager::create(const std::string& topicPrefix, Args&&... args) { - auto referenceManagerPtr = std::make_shared(std::forward(args)...); - return std::make_unique(topicPrefix, std::move(referenceManagerPtr)); +std::unique_ptr RosReferenceManager::create( + const std::string& topicPrefix, Args&&... args) { + auto referenceManagerPtr = + std::make_shared(std::forward(args)...); + return std::make_unique(topicPrefix, + std::move(referenceManagerPtr)); } } // namespace ocs2 diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h index b76dd4278..789afdada 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h @@ -29,10 +29,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include - #include +#include + namespace ocs2 { namespace ros { @@ -42,45 +42,67 @@ enum class CallbackInterpolationStrategy { }; /** - * Creates a ROS-based callback for SolverObserver that publishes a constraint term at the requested lookahead time points. + * Creates a ROS-based callback for SolverObserver that publishes a constraint + * term at the requested lookahead time points. * - * @param [in] nodeHandle: ROS node handle. - * @param [in] observingTimePoints: An array of lookahead times for which we want to publish the values of constraint. - * @param [in] topicNames: An array of topic names. For each observing time points, you should provide a unique topic name. - * @param [in] interpolationStrategy: The interpolation method used for acquiring data at each time point. - * @return A callback which can be set to SolverObserverModule in order to observe a requested term's constraint. + * @param [in] node: ROS node handle. + * @param [in] observingTimePoints: An array of lookahead times for which we + * want to publish the values of constraint. + * @param [in] topicNames: An array of topic names. For each observing time + * points, you should provide a unique topic name. + * @param [in] interpolationStrategy: The interpolation method used for + * acquiring data at each time point. + * @return A callback which can be set to SolverObserverModule in order to + * observe a requested term's constraint. */ SolverObserver::constraint_callback_t createConstraintCallback( - ::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, const std::vector& topicNames, - CallbackInterpolationStrategy interpolationStrategy = CallbackInterpolationStrategy::nearest_time); + const rclcpp::Node::SharedPtr& node, + const scalar_array_t& observingTimePoints, + const std::vector& topicNames, + CallbackInterpolationStrategy interpolationStrategy = + CallbackInterpolationStrategy::nearest_time); /** - * Creates a ROS-based callback for SolverObserver that publishes a term's LagrangianMetrics at the - * requested lookahead time points. + * Creates a ROS-based callback for SolverObserver that publishes a term's + * LagrangianMetrics at the requested lookahead time points. * - * @param [in] nodeHandle: ROS node handle. - * @param [in] observingTimePoints: An array of lookahead times for which we want to publish the values of LagrangianMetrics. - * @param [in] topicNames: An array of topic names. For each observing time points, you should provide a unique topic name. - * @param [in] interpolationStrategy: The interpolation method used for acquiring data at each time point. - * @return A callback which can be set to SolverObserverModule in order to observe a requested term's LagrangianMetrics. + * @param [in] node: ROS node handle. + * @param [in] observingTimePoints: An array of lookahead times for which we + * want to publish the values of LagrangianMetrics. + * @param [in] topicNames: An array of topic names. For each observing time + * points, you should provide a unique topic name. + * @param [in] interpolationStrategy: The interpolation method used for + * acquiring data at each time point. + * @return A callback which can be set to SolverObserverModule in order to + * observe a requested term's LagrangianMetrics. */ SolverObserver::lagrangian_callback_t createLagrangianCallback( - ::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, const std::vector& topicNames, - CallbackInterpolationStrategy interpolationStrategy = CallbackInterpolationStrategy::nearest_time); + const rclcpp::Node::SharedPtr& node, + const scalar_array_t& observingTimePoints, + const std::vector& topicNames, + CallbackInterpolationStrategy interpolationStrategy = + CallbackInterpolationStrategy::nearest_time); /** - * Creates a ROS-based callback for SolverObserver that publishes a term's Lagrange multiplier at the - * requested lookahead time points. + * Creates a ROS-based callback for SolverObserver that publishes a term's + * Lagrange multiplier at the requested lookahead time points. * - * @param [in] nodeHandle: ROS node handle. - * @param [in] observingTimePoints: An array of lookahead times for which we want to publish the values of multiplier. - * @param [in] topicNames: An array of topic names. For each observing time points, you should provide a unique topic name. - * @param [in] interpolationStrategy: The interpolation method used for acquiring data at each time point. - * @return A callback which can be set to SolverObserverModule in order to observe a requested term's multiplier. + * @param [in] node: ROS node handle. + * @param [in] observingTimePoints: An array of lookahead times for which we + * want to publish the values of multiplier. + * @param [in] topicNames: An array of topic names. For each observing time + * points, you should provide a unique topic name. + * @param [in] interpolationStrategy: The interpolation method used for + * acquiring data at each time point. + * @return A callback which can be set to SolverObserverModule in order to + * observe a requested term's multiplier. */ SolverObserver::multiplier_callback_t createMultiplierCallback( - ::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, const std::vector& topicNames, - CallbackInterpolationStrategy interpolationStrategy = CallbackInterpolationStrategy::nearest_time); + const rclcpp::Node::SharedPtr& node, + const scalar_array_t& observingTimePoints, + const std::vector& topicNames, + CallbackInterpolationStrategy interpolationStrategy = + CallbackInterpolationStrategy::nearest_time); } // namespace ros } // namespace ocs2 diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/visualization/VisualizationHelpers.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/visualization/VisualizationHelpers.h index 445d52dac..52d7f3f8d 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/visualization/VisualizationHelpers.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/visualization/VisualizationHelpers.h @@ -29,32 +29,33 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "ocs2_ros_interfaces/visualization/VisualizationColors.h" - -#include - #include #include +#include + +#include "ocs2_ros_interfaces/visualization/VisualizationColors.h" +#include // ROS messages -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include namespace ocs2 { -std_msgs::ColorRGBA getColor(Color color, double alpha = 1.0); +std_msgs::msg::ColorRGBA getColor(Color color, double alpha = 1.0); -void setVisible(visualization_msgs::Marker& marker); +void setVisible(visualization_msgs::msg::Marker& marker); -void setInvisible(visualization_msgs::Marker& marker); +void setInvisible(visualization_msgs::msg::Marker& marker); -std_msgs::Header getHeaderMsg(const std::string& frame_id, const ros::Time& timeStamp); +std_msgs::msg::Header getHeaderMsg(const std::string& frame_id, + const rclcpp::Time& timeStamp); template -void assignHeader(It firstIt, It lastIt, const std_msgs::Header& header) { +void assignHeader(It firstIt, It lastIt, const std_msgs::msg::Header& header) { for (; firstIt != lastIt; ++firstIt) { firstIt->header = header; } @@ -67,34 +68,49 @@ void assignIncreasingId(It firstIt, It lastIt, int startId = 0) { } } -visualization_msgs::Marker getLineMsg(std::vector&& points, Color color, double lineWidth); +visualization_msgs::msg::Marker getLineMsg( + std::vector&& points, Color color, + double lineWidth); -geometry_msgs::Point getPointMsg(const Eigen::Vector3d& point); +geometry_msgs::msg::Point getPointMsg(const Eigen::Vector3d& point); -geometry_msgs::Vector3 getVectorMsg(const Eigen::Vector3d& vec); +geometry_msgs::msg::Vector3 getVectorMsg(const Eigen::Vector3d& vec); -geometry_msgs::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientation); +geometry_msgs::msg::Quaternion getOrientationMsg( + const Eigen::Quaterniond& orientation); -visualization_msgs::Marker getSphereMsg(const Eigen::Vector3d& point, Color color, double diameter); +visualization_msgs::msg::Marker getSphereMsg(const Eigen::Vector3d& point, + Color color, double diameter); -visualization_msgs::Marker getPlaneMsg(const Eigen::Vector3d& point, const Eigen::Quaterniond& orientation, Color color, double width, - double length, double thickness); +visualization_msgs::msg::Marker getPlaneMsg( + const Eigen::Vector3d& point, const Eigen::Quaterniond& orientation, + Color color, double width, double length, double thickness); -visualization_msgs::Marker getArrowToPointMsg(const Eigen::Vector3d& vec, const Eigen::Vector3d& point, Color color); +visualization_msgs::msg::Marker getArrowToPointMsg(const Eigen::Vector3d& vec, + const Eigen::Vector3d& point, + Color color); -visualization_msgs::Marker getArrowAtPointMsg(const Eigen::Vector3d& vec, const Eigen::Vector3d& point, Color color); +visualization_msgs::msg::Marker getArrowAtPointMsg(const Eigen::Vector3d& vec, + const Eigen::Vector3d& point, + Color color); -visualization_msgs::Marker getArrowBetweenPointsMsg(const Eigen::Vector3d& start, const Eigen::Vector3d& end, Color color); +visualization_msgs::msg::Marker getArrowBetweenPointsMsg( + const Eigen::Vector3d& start, const Eigen::Vector3d& end, Color color); -visualization_msgs::Marker getFootMarker(const Eigen::Vector3d& position, bool contactFlag, Color color, double diameter, - double liftedAlpha); +visualization_msgs::msg::Marker getFootMarker(const Eigen::Vector3d& position, + bool contactFlag, Color color, + double diameter, + double liftedAlpha); -visualization_msgs::Marker getForceMarker(const Eigen::Vector3d& force, const Eigen::Vector3d& position, bool contactFlag, Color color, - double forceScale); +visualization_msgs::msg::Marker getForceMarker(const Eigen::Vector3d& force, + const Eigen::Vector3d& position, + bool contactFlag, Color color, + double forceScale); template -visualization_msgs::Marker getCenterOfPressureMarker(ForceIt firstForce, ForceIt lastForce, PositionIt positionIt, ContactIt contactIt, - Color color, double diameter) { +visualization_msgs::msg::Marker getCenterOfPressureMarker( + ForceIt firstForce, ForceIt lastForce, PositionIt positionIt, + ContactIt contactIt, Color color, double diameter) { // Compute center of pressure Eigen::Vector3d centerOfPressure = Eigen::Vector3d::Zero(); double sum_z = 0.0; @@ -109,7 +125,8 @@ visualization_msgs::Marker getCenterOfPressureMarker(ForceIt firstForce, ForceIt } // Construct marker - visualization_msgs::Marker copMarker = getSphereMsg(centerOfPressure, color, diameter); + visualization_msgs::msg::Marker copMarker = + getSphereMsg(centerOfPressure, color, diameter); if (numContacts == 0) { setInvisible(copMarker); } @@ -119,12 +136,16 @@ visualization_msgs::Marker getCenterOfPressureMarker(ForceIt firstForce, ForceIt } template -visualization_msgs::Marker getSupportPolygonMarker(PositionIt firstPos, PositionIt lastPos, ContactIt contactIt, Color color, - double lineWidth) { - visualization_msgs::Marker lineList; - lineList.type = visualization_msgs::Marker::LINE_LIST; +visualization_msgs::msg::Marker getSupportPolygonMarker(PositionIt firstPos, + PositionIt lastPos, + ContactIt contactIt, + Color color, + double lineWidth) { + visualization_msgs::msg::Marker lineList; + lineList.type = visualization_msgs::msg::Marker::LINE_LIST; auto numElements = std::distance(firstPos, lastPos); - lineList.points.reserve(numElements * (numElements - 1) / 2); // Upper bound on the number of lines + lineList.points.reserve(numElements * (numElements - 1) / + 2); // Upper bound on the number of lines // Loop over all positions for (; firstPos != lastPos; ++firstPos, ++contactIt) { @@ -133,7 +154,8 @@ visualization_msgs::Marker getSupportPolygonMarker(PositionIt firstPos, Position auto nextContact = std::next(contactIt); for (; nextPos != lastPos; ++nextPos, ++nextContact) { if (*contactIt && *nextContact) { - // When positions are both marked as in contact, draw a line between the two points + // When positions are both marked as in contact, draw a line between the + // two points lineList.points.push_back(getPointMsg(*firstPos)); lineList.points.push_back(getPointMsg(*nextPos)); } @@ -151,7 +173,9 @@ double timedExecutionInSeconds(F func) { auto start = std::chrono::steady_clock::now(); func(); auto finish = std::chrono::steady_clock::now(); - return std::chrono::duration_cast>(finish - start).count(); + return std::chrono::duration_cast>(finish - + start) + .count(); } } // namespace ocs2 diff --git a/ocs2_ros_interfaces/package.xml b/ocs2_ros_interfaces/package.xml index 3398852f0..af9563d9a 100644 --- a/ocs2_ros_interfaces/package.xml +++ b/ocs2_ros_interfaces/package.xml @@ -1,6 +1,6 @@ - - + + ocs2_ros_interfaces 0.0.0 @@ -11,12 +11,15 @@ Jan Carius Ruben Grandia - catkin + ament_cmake + eigen3_cmake_module + eigen3_cmake_module cmake_modules cmake_clang_tools - roscpp + eigen + rclcpp ocs2_msgs ocs2_core ocs2_mpc @@ -25,5 +28,10 @@ geometry_msgs interactive_markers rqt_multiplot + ros2launch + + + ament_cmake + diff --git a/ocs2_ros_interfaces/src/command/TargetTrajectoriesInteractiveMarker.cpp b/ocs2_ros_interfaces/src/command/TargetTrajectoriesInteractiveMarker.cpp index 264a59f0f..c2ef1ad0e 100644 --- a/ocs2_ros_interfaces/src/command/TargetTrajectoriesInteractiveMarker.cpp +++ b/ocs2_ros_interfaces/src/command/TargetTrajectoriesInteractiveMarker.cpp @@ -29,36 +29,50 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ros_interfaces/command/TargetTrajectoriesInteractiveMarker.h" -#include #include +#include + namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -TargetTrajectoriesInteractiveMarker::TargetTrajectoriesInteractiveMarker(::ros::NodeHandle& nodeHandle, const std::string& topicPrefix, - GaolPoseToTargetTrajectories gaolPoseToTargetTrajectories) - : server_("simple_marker"), gaolPoseToTargetTrajectories_(std::move(gaolPoseToTargetTrajectories)) { +TargetTrajectoriesInteractiveMarker::TargetTrajectoriesInteractiveMarker( + const rclcpp::Node::SharedPtr& node, const std::string& topicPrefix, + GaolPoseToTargetTrajectories gaolPoseToTargetTrajectories) + : node_(node), + server_("simple_marker", node_), + gaolPoseToTargetTrajectories_(std::move(gaolPoseToTargetTrajectories)) { // observation subscriber - auto observationCallback = [this](const ocs2_msgs::mpc_observation::ConstPtr& msg) { - std::lock_guard lock(latestObservationMutex_); - latestObservation_ = ros_msg_conversions::readObservationMsg(*msg); - }; - observationSubscriber_ = nodeHandle.subscribe(topicPrefix + "_mpc_observation", 1, observationCallback); + auto observationCallback = + [this](const ocs2_msgs::msg::MpcObservation::ConstSharedPtr& msg) { + std::lock_guard lock(latestObservationMutex_); + latestObservation_ = ros_msg_conversions::readObservationMsg(*msg); + }; + observationSubscriber_ = + node_->create_subscription( + topicPrefix + "_mpc_observation", 1, observationCallback); // Trajectories publisher - targetTrajectoriesPublisherPtr_.reset(new TargetTrajectoriesRosPublisher(nodeHandle, topicPrefix)); + targetTrajectoriesPublisherPtr_.reset( + new TargetTrajectoriesRosPublisher(node_, topicPrefix)); // create an interactive marker for our server - menuHandler_.insert("Send target pose", boost::bind(&TargetTrajectoriesInteractiveMarker::processFeedback, this, _1)); + auto feedback_cb = + [&](const visualization_msgs::msg::InteractiveMarkerFeedback:: + ConstSharedPtr& feedback) { processFeedback(feedback); }; + menuHandler_.insert("Send target pose", feedback_cb); // create an interactive marker for our server auto interactiveMarker = createInteractiveMarker(); // add the interactive marker to our collection & // tell the server to call processFeedback() when feedback arrives for it - server_.insert(interactiveMarker); //, boost::bind(&TargetTrajectoriesInteractiveMarker::processFeedback, this, _1)); + server_.insert( + interactiveMarker); //, + // boost::bind(&TargetTrajectoriesInteractiveMarker::processFeedback, + // this, _1)); menuHandler_.apply(server_, interactiveMarker.name); // 'commit' changes and send to all clients @@ -68,10 +82,11 @@ TargetTrajectoriesInteractiveMarker::TargetTrajectoriesInteractiveMarker(::ros:: /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -visualization_msgs::InteractiveMarker TargetTrajectoriesInteractiveMarker::createInteractiveMarker() const { - visualization_msgs::InteractiveMarker interactiveMarker; +visualization_msgs::msg::InteractiveMarker +TargetTrajectoriesInteractiveMarker::createInteractiveMarker() const { + visualization_msgs::msg::InteractiveMarker interactiveMarker; interactiveMarker.header.frame_id = "world"; - interactiveMarker.header.stamp = ros::Time::now(); + interactiveMarker.header.stamp = node_->now(); interactiveMarker.name = "Goal"; interactiveMarker.scale = 0.2; interactiveMarker.description = "Right click to send command"; @@ -79,8 +94,8 @@ visualization_msgs::InteractiveMarker TargetTrajectoriesInteractiveMarker::creat // create a grey box marker const auto boxMarker = []() { - visualization_msgs::Marker marker; - marker.type = visualization_msgs::Marker::CUBE; + visualization_msgs::msg::Marker marker; + marker.type = visualization_msgs::msg::Marker::CUBE; marker.scale.x = 0.1; marker.scale.y = 0.1; marker.scale.z = 0.1; @@ -92,10 +107,11 @@ visualization_msgs::InteractiveMarker TargetTrajectoriesInteractiveMarker::creat }(); // create a non-interactive control which contains the box - visualization_msgs::InteractiveMarkerControl boxControl; + visualization_msgs::msg::InteractiveMarkerControl boxControl; boxControl.always_visible = 1; boxControl.markers.push_back(boxMarker); - boxControl.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D; + boxControl.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::MOVE_ROTATE_3D; // add the control to the interactive marker interactiveMarker.controls.push_back(boxControl); @@ -103,17 +119,19 @@ visualization_msgs::InteractiveMarker TargetTrajectoriesInteractiveMarker::creat // create a control which will move the box // this control does not contain any markers, // which will cause RViz to insert two arrows - visualization_msgs::InteractiveMarkerControl control; + visualization_msgs::msg::InteractiveMarkerControl control; control.orientation.w = 1; control.orientation.x = 1; control.orientation.y = 0; control.orientation.z = 0; control.name = "rotate_x"; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; + control.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS; interactiveMarker.controls.push_back(control); control.name = "move_x"; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; + control.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS; interactiveMarker.controls.push_back(control); control.orientation.w = 1; @@ -121,10 +139,12 @@ visualization_msgs::InteractiveMarker TargetTrajectoriesInteractiveMarker::creat control.orientation.y = 1; control.orientation.z = 0; control.name = "rotate_z"; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; + control.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS; interactiveMarker.controls.push_back(control); control.name = "move_z"; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; + control.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS; interactiveMarker.controls.push_back(control); control.orientation.w = 1; @@ -132,10 +152,12 @@ visualization_msgs::InteractiveMarker TargetTrajectoriesInteractiveMarker::creat control.orientation.y = 0; control.orientation.z = 1; control.name = "rotate_y"; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; + control.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS; interactiveMarker.controls.push_back(control); control.name = "move_y"; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; + control.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS; interactiveMarker.controls.push_back(control); return interactiveMarker; @@ -144,11 +166,16 @@ visualization_msgs::InteractiveMarker TargetTrajectoriesInteractiveMarker::creat /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void TargetTrajectoriesInteractiveMarker::processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) { +void TargetTrajectoriesInteractiveMarker::processFeedback( + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& + feedback) { // Desired state trajectory - const Eigen::Vector3d position(feedback->pose.position.x, feedback->pose.position.y, feedback->pose.position.z); - const Eigen::Quaterniond orientation(feedback->pose.orientation.w, feedback->pose.orientation.x, feedback->pose.orientation.y, - feedback->pose.orientation.z); + const Eigen::Vector3d position(feedback->pose.position.x, + feedback->pose.position.y, + feedback->pose.position.z); + const Eigen::Quaterniond orientation( + feedback->pose.orientation.w, feedback->pose.orientation.x, + feedback->pose.orientation.y, feedback->pose.orientation.z); // get the latest observation SystemObservation observation; @@ -158,10 +185,12 @@ void TargetTrajectoriesInteractiveMarker::processFeedback(const visualization_ms } // get TargetTrajectories - const auto targetTrajectories = gaolPoseToTargetTrajectories_(position, orientation, observation); + const auto targetTrajectories = + gaolPoseToTargetTrajectories_(position, orientation, observation); // publish TargetTrajectories - targetTrajectoriesPublisherPtr_->publishTargetTrajectories(targetTrajectories); + targetTrajectoriesPublisherPtr_->publishTargetTrajectories( + targetTrajectories); } } // namespace ocs2 diff --git a/ocs2_ros_interfaces/src/command/TargetTrajectoriesKeyboardPublisher.cpp b/ocs2_ros_interfaces/src/command/TargetTrajectoriesKeyboardPublisher.cpp index df30f83d9..c6652df01 100644 --- a/ocs2_ros_interfaces/src/command/TargetTrajectoriesKeyboardPublisher.cpp +++ b/ocs2_ros_interfaces/src/command/TargetTrajectoriesKeyboardPublisher.cpp @@ -31,44 +31,57 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include #include +#include + namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -TargetTrajectoriesKeyboardPublisher::TargetTrajectoriesKeyboardPublisher(::ros::NodeHandle& nodeHandle, const std::string& topicPrefix, - const scalar_array_t& targetCommandLimits, - CommandLineToTargetTrajectories commandLineToTargetTrajectoriesFun) - : targetCommandLimits_(Eigen::Map(targetCommandLimits.data(), targetCommandLimits.size())), - commandLineToTargetTrajectoriesFun_(std::move(commandLineToTargetTrajectoriesFun)) { +TargetTrajectoriesKeyboardPublisher::TargetTrajectoriesKeyboardPublisher( + const rclcpp::Node::SharedPtr& node, const std::string& topicPrefix, + const scalar_array_t& targetCommandLimits, + CommandLineToTargetTrajectories commandLineToTargetTrajectoriesFun) + : node_(node), + targetCommandLimits_(Eigen::Map( + targetCommandLimits.data(), targetCommandLimits.size())), + commandLineToTargetTrajectoriesFun_( + std::move(commandLineToTargetTrajectoriesFun)) { // observation subscriber - auto observationCallback = [this](const ocs2_msgs::mpc_observation::ConstPtr& msg) { - std::lock_guard lock(latestObservationMutex_); - latestObservation_ = ros_msg_conversions::readObservationMsg(*msg); - }; - observationSubscriber_ = nodeHandle.subscribe(topicPrefix + "_mpc_observation", 1, observationCallback); + auto observationCallback = + [this](const ocs2_msgs::msg::MpcObservation::ConstSharedPtr& msg) { + std::lock_guard lock(latestObservationMutex_); + latestObservation_ = ros_msg_conversions::readObservationMsg(*msg); + }; + observationSubscriber_ = + node->create_subscription( + topicPrefix + "_mpc_observation", 1, observationCallback); // Trajectories publisher - targetTrajectoriesPublisherPtr_.reset(new TargetTrajectoriesRosPublisher(nodeHandle, topicPrefix)); + targetTrajectoriesPublisherPtr_.reset( + new TargetTrajectoriesRosPublisher(node, topicPrefix)); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void TargetTrajectoriesKeyboardPublisher::publishKeyboardCommand(const std::string& commadMsg) { - while (ros::ok() && ros::master::check()) { +void TargetTrajectoriesKeyboardPublisher::publishKeyboardCommand( + const std::string& commadMsg) { + while (rclcpp::ok()) { // get command line std::cout << commadMsg << ": "; - const vector_t commandLineInput = getCommandLine().cwiseMin(targetCommandLimits_).cwiseMax(-targetCommandLimits_); + const vector_t commandLineInput = getCommandLine() + .cwiseMin(targetCommandLimits_) + .cwiseMax(-targetCommandLimits_); // display - std::cout << "The following command is published: [" << toDelimitedString(commandLineInput) << "]\n\n"; + std::cout << "The following command is published: [" + << toDelimitedString(commandLineInput) << "]\n\n"; // get the latest observation - ::ros::spinOnce(); + rclcpp::spin_some(node_); SystemObservation observation; { std::lock_guard lock(latestObservationMutex_); @@ -76,10 +89,12 @@ void TargetTrajectoriesKeyboardPublisher::publishKeyboardCommand(const std::stri } // get TargetTrajectories - const auto targetTrajectories = commandLineToTargetTrajectoriesFun_(commandLineInput, observation); + const auto targetTrajectories = + commandLineToTargetTrajectoriesFun_(commandLineInput, observation); // publish TargetTrajectories - targetTrajectoriesPublisherPtr_->publishTargetTrajectories(targetTrajectories); + targetTrajectoriesPublisherPtr_->publishTargetTrajectories( + targetTrajectories); } // end of while loop } @@ -88,7 +103,7 @@ void TargetTrajectoriesKeyboardPublisher::publishKeyboardCommand(const std::stri /******************************************************************************************************/ vector_t TargetTrajectoriesKeyboardPublisher::getCommandLine() { // get command line as one long string - auto shouldTerminate = []() { return !ros::ok() || !ros::master::check(); }; + auto shouldTerminate = []() { return !rclcpp::ok(); }; const std::string line = getCommandLineString(shouldTerminate); // a line to words diff --git a/ocs2_ros_interfaces/src/command/TargetTrajectoriesRosPublisher.cpp b/ocs2_ros_interfaces/src/command/TargetTrajectoriesRosPublisher.cpp index 832bac294..e51ae8ff7 100644 --- a/ocs2_ros_interfaces/src/command/TargetTrajectoriesRosPublisher.cpp +++ b/ocs2_ros_interfaces/src/command/TargetTrajectoriesRosPublisher.cpp @@ -30,32 +30,32 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ros_interfaces/command/TargetTrajectoriesRosPublisher.h" // MPC messages -#include +#include namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -TargetTrajectoriesRosPublisher::TargetTrajectoriesRosPublisher(::ros::NodeHandle& nodeHandle, const std::string& topicPrefix) { - targetTrajectoriesPublisher_ = nodeHandle.advertise(topicPrefix + "_mpc_target", 1, false); - ros::spinOnce(); - ROS_INFO_STREAM("The TargetTrajectories is publishing on " + topicPrefix + "_mpc_target topic."); +TargetTrajectoriesRosPublisher::TargetTrajectoriesRosPublisher(rclcpp::Node::SharedPtr node, const std::string& topicPrefix) : node_(node) { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node_); + targetTrajectoriesPublisher_ = node_->create_publisher(topicPrefix + "_mpc_target", 1); + executor.spin_some(); + RCLCPP_INFO(node_->get_logger(), "The TargetTrajectories is publishing on %s_mpc_target topic.", topicPrefix.c_str()); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -TargetTrajectoriesRosPublisher::~TargetTrajectoriesRosPublisher() { - targetTrajectoriesPublisher_.shutdown(); -} +TargetTrajectoriesRosPublisher::~TargetTrajectoriesRosPublisher() {} /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ void TargetTrajectoriesRosPublisher::publishTargetTrajectories(const TargetTrajectories& targetTrajectories) { const auto mpcTargetTrajectoriesMsg = ros_msg_conversions::createTargetTrajectoriesMsg(targetTrajectories); - targetTrajectoriesPublisher_.publish(mpcTargetTrajectoriesMsg); + targetTrajectoriesPublisher_->publish(mpcTargetTrajectoriesMsg); } } // namespace ocs2 diff --git a/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp b/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp index 7c5e82bcd..9ade81948 100644 --- a/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp +++ b/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp @@ -35,8 +35,9 @@ namespace ros_msg_conversions { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::mpc_observation createObservationMsg(const SystemObservation& observation) { - ocs2_msgs::mpc_observation observationMsg; +ocs2_msgs::msg::MpcObservation createObservationMsg( + const SystemObservation& observation) { + ocs2_msgs::msg::MpcObservation observationMsg; observationMsg.time = observation.time; @@ -58,16 +59,21 @@ ocs2_msgs::mpc_observation createObservationMsg(const SystemObservation& observa /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -SystemObservation readObservationMsg(const ocs2_msgs::mpc_observation& observationMsg) { +SystemObservation readObservationMsg( + const ocs2_msgs::msg::MpcObservation& observationMsg) { SystemObservation observation; observation.time = observationMsg.time; const auto& state = observationMsg.state.value; - observation.state = Eigen::Map(state.data(), state.size()).cast(); + observation.state = + Eigen::Map(state.data(), state.size()) + .cast(); const auto& input = observationMsg.input.value; - observation.input = Eigen::Map(input.data(), input.size()).cast(); + observation.input = + Eigen::Map(input.data(), input.size()) + .cast(); observation.mode = observationMsg.mode; @@ -77,20 +83,21 @@ SystemObservation readObservationMsg(const ocs2_msgs::mpc_observation& observati /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::mode_schedule createModeScheduleMsg(const ModeSchedule& modeSchedule) { - ocs2_msgs::mode_schedule modeScheduleMsg; +ocs2_msgs::msg::ModeSchedule createModeScheduleMsg( + const ModeSchedule& modeSchedule) { + ocs2_msgs::msg::ModeSchedule modeScheduleMsg; // event times - modeScheduleMsg.eventTimes.clear(); - modeScheduleMsg.eventTimes.reserve(modeSchedule.eventTimes.size()); + modeScheduleMsg.event_times.clear(); + modeScheduleMsg.event_times.reserve(modeSchedule.eventTimes.size()); for (const auto& ti : modeSchedule.eventTimes) { - modeScheduleMsg.eventTimes.push_back(ti); + modeScheduleMsg.event_times.push_back(ti); } // mode sequence - modeScheduleMsg.modeSequence.clear(); - modeScheduleMsg.modeSequence.reserve(modeSchedule.modeSequence.size()); + modeScheduleMsg.mode_sequence.clear(); + modeScheduleMsg.mode_sequence.reserve(modeSchedule.modeSequence.size()); for (const auto& si : modeSchedule.modeSequence) { - modeScheduleMsg.modeSequence.push_back(si); + modeScheduleMsg.mode_sequence.push_back(si); } return modeScheduleMsg; @@ -99,37 +106,44 @@ ocs2_msgs::mode_schedule createModeScheduleMsg(const ModeSchedule& modeSchedule) /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ModeSchedule readModeScheduleMsg(const ocs2_msgs::mode_schedule& modeScheduleMsg) { +ModeSchedule readModeScheduleMsg( + const ocs2_msgs::msg::ModeSchedule& modeScheduleMsg) { // event times scalar_array_t eventTimes; - eventTimes.reserve(modeScheduleMsg.eventTimes.size()); - for (const auto& ti : modeScheduleMsg.eventTimes) { + eventTimes.reserve(modeScheduleMsg.event_times.size()); + for (const auto& ti : modeScheduleMsg.event_times) { eventTimes.push_back(ti); } // mode sequence - size_array_t modeSequence; - modeSequence.reserve(modeScheduleMsg.modeSequence.size()); - for (const auto& si : modeScheduleMsg.modeSequence) { - modeSequence.push_back(si); + size_array_t mode_sequence; + mode_sequence.reserve(modeScheduleMsg.mode_sequence.size()); + for (const auto& si : modeScheduleMsg.mode_sequence) { + mode_sequence.push_back(si); } - return {eventTimes, modeSequence}; + return {eventTimes, mode_sequence}; } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::mpc_performance_indices createPerformanceIndicesMsg(scalar_t initTime, const PerformanceIndex& performanceIndices) { - ocs2_msgs::mpc_performance_indices performanceIndicesMsg; +ocs2_msgs::msg::MpcPerformanceIndices createPerformanceIndicesMsg( + scalar_t initTime, + const PerformanceIndex& performanceIndices) { + ocs2_msgs::msg::MpcPerformanceIndices performanceIndicesMsg; - performanceIndicesMsg.initTime = initTime; + performanceIndicesMsg.init_time = initTime; performanceIndicesMsg.merit = performanceIndices.merit; performanceIndicesMsg.cost = performanceIndices.cost; - performanceIndicesMsg.dynamicsViolationSSE = performanceIndices.dynamicsViolationSSE; - performanceIndicesMsg.equalityConstraintsSSE = performanceIndices.equalityConstraintsSSE; - performanceIndicesMsg.equalityLagrangian = performanceIndices.equalityLagrangian; - performanceIndicesMsg.inequalityLagrangian = performanceIndices.inequalityLagrangian; + performanceIndicesMsg.dynamics_violation_sse = + performanceIndices.dynamicsViolationSSE; + performanceIndicesMsg.equality_constraints_sse = + performanceIndices.equalityConstraintsSSE; + performanceIndicesMsg.equality_lagrangian = + performanceIndices.equalityLagrangian; + performanceIndicesMsg.inequality_lagrangian = + performanceIndices.inequalityLagrangian; return performanceIndicesMsg; } @@ -137,15 +151,20 @@ ocs2_msgs::mpc_performance_indices createPerformanceIndicesMsg(scalar_t initTime /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -PerformanceIndex readPerformanceIndicesMsg(const ocs2_msgs::mpc_performance_indices& performanceIndicesMsg) { +PerformanceIndex readPerformanceIndicesMsg( + const ocs2_msgs::msg::MpcPerformanceIndices& performanceIndicesMsg) { PerformanceIndex performanceIndices; performanceIndices.merit = performanceIndicesMsg.merit; performanceIndices.cost = performanceIndicesMsg.cost; - performanceIndices.dynamicsViolationSSE = performanceIndicesMsg.dynamicsViolationSSE; - performanceIndices.equalityConstraintsSSE = performanceIndicesMsg.equalityConstraintsSSE; - performanceIndices.equalityLagrangian = performanceIndicesMsg.equalityLagrangian; - performanceIndices.inequalityLagrangian = performanceIndicesMsg.inequalityLagrangian; + performanceIndices.dynamicsViolationSSE = + performanceIndicesMsg.dynamics_violation_sse; + performanceIndices.equalityConstraintsSSE = + performanceIndicesMsg.equality_constraints_sse; + performanceIndices.equalityLagrangian = + performanceIndicesMsg.equality_lagrangian; + performanceIndices.inequalityLagrangian = + performanceIndicesMsg.inequality_lagrangian; return performanceIndices; } @@ -153,29 +172,32 @@ PerformanceIndex readPerformanceIndicesMsg(const ocs2_msgs::mpc_performance_indi /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::mpc_target_trajectories createTargetTrajectoriesMsg(const TargetTrajectories& targetTrajectories) { - ocs2_msgs::mpc_target_trajectories targetTrajectoriesMsg; +ocs2_msgs::msg::MpcTargetTrajectories createTargetTrajectoriesMsg( + const TargetTrajectories& targetTrajectories) { + ocs2_msgs::msg::MpcTargetTrajectories targetTrajectoriesMsg; const auto& timeTrajectory = targetTrajectories.timeTrajectory; const auto& stateTrajectory = targetTrajectories.stateTrajectory; const auto& inputTrajectory = targetTrajectories.inputTrajectory; // time and state size_t N = stateTrajectory.size(); - targetTrajectoriesMsg.timeTrajectory.resize(N); - targetTrajectoriesMsg.stateTrajectory.resize(N); + targetTrajectoriesMsg.time_trajectory.resize(N); + targetTrajectoriesMsg.state_trajectory.resize(N); for (size_t i = 0; i < N; i++) { - targetTrajectoriesMsg.timeTrajectory[i] = timeTrajectory[i]; + targetTrajectoriesMsg.time_trajectory[i] = timeTrajectory[i]; - targetTrajectoriesMsg.stateTrajectory[i].value = - std::vector(stateTrajectory[i].data(), stateTrajectory[i].data() + stateTrajectory[i].size()); + targetTrajectoriesMsg.state_trajectory[i].value = std::vector( + stateTrajectory[i].data(), + stateTrajectory[i].data() + stateTrajectory[i].size()); } // end of i loop // input N = inputTrajectory.size(); - targetTrajectoriesMsg.inputTrajectory.resize(N); + targetTrajectoriesMsg.input_trajectory.resize(N); for (size_t i = 0; i < N; i++) { - targetTrajectoriesMsg.inputTrajectory[i].value = - std::vector(inputTrajectory[i].data(), inputTrajectory[i].data() + inputTrajectory[i].size()); + targetTrajectoriesMsg.input_trajectory[i].value = std::vector( + inputTrajectory[i].data(), + inputTrajectory[i].data() + inputTrajectory[i].size()); } // end of i loop return targetTrajectoriesMsg; @@ -184,40 +206,48 @@ ocs2_msgs::mpc_target_trajectories createTargetTrajectoriesMsg(const TargetTraje /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -TargetTrajectories readTargetTrajectoriesMsg(const ocs2_msgs::mpc_target_trajectories& targetTrajectoriesMsg) { - size_t N = targetTrajectoriesMsg.stateTrajectory.size(); +TargetTrajectories readTargetTrajectoriesMsg( + const ocs2_msgs::msg::MpcTargetTrajectories& targetTrajectoriesMsg) { + size_t N = targetTrajectoriesMsg.state_trajectory.size(); if (N == 0) { - throw std::runtime_error("An empty target trajectories message is received."); + throw std::runtime_error( + "An empty target trajectories message is received."); } // state and time scalar_array_t desiredTimeTrajectory(N); vector_array_t desiredStateTrajectory(N); for (size_t i = 0; i < N; i++) { - desiredTimeTrajectory[i] = targetTrajectoriesMsg.timeTrajectory[i]; + desiredTimeTrajectory[i] = targetTrajectoriesMsg.time_trajectory[i]; - desiredStateTrajectory[i] = Eigen::Map(targetTrajectoriesMsg.stateTrajectory[i].value.data(), - targetTrajectoriesMsg.stateTrajectory[i].value.size()) - .cast(); + desiredStateTrajectory[i] = + Eigen::Map( + targetTrajectoriesMsg.state_trajectory[i].value.data(), + targetTrajectoriesMsg.state_trajectory[i].value.size()) + .cast(); } // end of i loop // input - N = targetTrajectoriesMsg.inputTrajectory.size(); + N = targetTrajectoriesMsg.input_trajectory.size(); vector_array_t desiredInputTrajectory(N); for (size_t i = 0; i < N; i++) { - desiredInputTrajectory[i] = Eigen::Map(targetTrajectoriesMsg.inputTrajectory[i].value.data(), - targetTrajectoriesMsg.inputTrajectory[i].value.size()) - .cast(); + desiredInputTrajectory[i] = + Eigen::Map( + targetTrajectoriesMsg.input_trajectory[i].value.data(), + targetTrajectoriesMsg.input_trajectory[i].value.size()) + .cast(); } // end of i loop - return {desiredTimeTrajectory, desiredStateTrajectory, desiredInputTrajectory}; + return {desiredTimeTrajectory, desiredStateTrajectory, + desiredInputTrajectory}; } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::constraint createConstraintMsg(scalar_t time, const vector_t& constraint) { - ocs2_msgs::constraint constraintMsg; +ocs2_msgs::msg::Constraint createConstraintMsg(scalar_t time, + const vector_t& constraint) { + ocs2_msgs::msg::Constraint constraintMsg; constraintMsg.time = time; constraintMsg.value.resize(constraint.size()); @@ -231,8 +261,10 @@ ocs2_msgs::constraint createConstraintMsg(scalar_t time, const vector_t& constra /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::lagrangian_metrics createLagrangianMetricsMsg(scalar_t time, LagrangianMetricsConstRef metrics) { - ocs2_msgs::lagrangian_metrics metricsMsg; +ocs2_msgs::msg::LagrangianMetrics createLagrangianMetricsMsg( + scalar_t time, + LagrangianMetricsConstRef metrics) { + ocs2_msgs::msg::LagrangianMetrics metricsMsg; metricsMsg.time = time; metricsMsg.penalty = metrics.penalty; @@ -248,8 +280,9 @@ ocs2_msgs::lagrangian_metrics createLagrangianMetricsMsg(scalar_t time, Lagrangi /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::multiplier createMultiplierMsg(scalar_t time, MultiplierConstRef multiplier) { - ocs2_msgs::multiplier multiplierMsg; +ocs2_msgs::msg::Multiplier createMultiplierMsg(scalar_t time, + MultiplierConstRef multiplier) { + ocs2_msgs::msg::Multiplier multiplierMsg; multiplierMsg.time = time; multiplierMsg.penalty = multiplier.penalty; diff --git a/ocs2_ros_interfaces/src/common/RosMsgHelpers.cpp b/ocs2_ros_interfaces/src/common/RosMsgHelpers.cpp index 2d716dbd8..7831d399f 100644 --- a/ocs2_ros_interfaces/src/common/RosMsgHelpers.cpp +++ b/ocs2_ros_interfaces/src/common/RosMsgHelpers.cpp @@ -32,24 +32,24 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace ros_msg_helpers { -geometry_msgs::Point getPointMsg(const Eigen::Vector3d& point) { - geometry_msgs::Point pointMsg; +geometry_msgs::msg::Point getPointMsg(const Eigen::Vector3d& point) { + geometry_msgs::msg::Point pointMsg; pointMsg.x = point.x(); pointMsg.y = point.y(); pointMsg.z = point.z(); return pointMsg; } -geometry_msgs::Vector3 getVectorMsg(const Eigen::Vector3d& vec) { - geometry_msgs::Vector3 vecMsg; +geometry_msgs::msg::Vector3 getVectorMsg(const Eigen::Vector3d& vec) { + geometry_msgs::msg::Vector3 vecMsg; vecMsg.x = vec.x(); vecMsg.y = vec.y(); vecMsg.z = vec.z(); return vecMsg; } -geometry_msgs::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientation) { - geometry_msgs::Quaternion orientationMsg; +geometry_msgs::msg::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientation) { + geometry_msgs::msg::Quaternion orientationMsg; orientationMsg.x = orientation.x(); orientationMsg.y = orientation.y(); orientationMsg.z = orientation.z(); @@ -57,16 +57,16 @@ geometry_msgs::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientatio return orientationMsg; } -std_msgs::Header getHeaderMsg(const std::string& frame_id, const ros::Time& timeStamp) { - std_msgs::Header header; +std_msgs::msg::Header getHeaderMsg(const std::string& frame_id, const rclcpp::Time& timeStamp) { + std_msgs::msg::Header header; header.frame_id = frame_id; header.stamp = timeStamp; return header; } -visualization_msgs::Marker getLineMsg(std::vector&& points, std::array color, double lineWidth) { - visualization_msgs::Marker line; - line.type = visualization_msgs::Marker::LINE_STRIP; +visualization_msgs::msg::Marker getLineMsg(std::vector&& points, std::array color, double lineWidth) { + visualization_msgs::msg::Marker line; + line.type = visualization_msgs::msg::Marker::LINE_STRIP; line.scale.x = lineWidth; line.color = getColor(color); line.points = std::move(points); @@ -74,8 +74,8 @@ visualization_msgs::Marker getLineMsg(std::vector&& points return line; } -std_msgs::ColorRGBA getColor(std::array rgb, double alpha /* = 1.0*/) { - std_msgs::ColorRGBA colorMsg; +std_msgs::msg::ColorRGBA getColor(std::array rgb, double alpha /* = 1.0*/) { + std_msgs::msg::ColorRGBA colorMsg; colorMsg.r = rgb[0]; colorMsg.g = rgb[1]; colorMsg.b = rgb[2]; diff --git a/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp b/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp index 4178bb1bb..24b8ba3d2 100644 --- a/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp +++ b/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp @@ -31,6 +31,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ros_interfaces/common/RosMsgConversions.h" +const rclcpp::Logger LOGGER = rclcpp::get_logger("MPC_ROS_Interface"); + namespace ocs2 { /******************************************************************************************************/ @@ -54,17 +56,17 @@ MPC_ROS_Interface::MPC_ROS_Interface(MPC_BASE& mpc, std::string topicPrefix) /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MPC_ROS_Interface::~MPC_ROS_Interface() { - shutdownNode(); -} +MPC_ROS_Interface::~MPC_ROS_Interface() { shutdownNode(); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MPC_ROS_Interface::resetMpcNode(TargetTrajectories&& initTargetTrajectories) { +void MPC_ROS_Interface::resetMpcNode( + TargetTrajectories&& initTargetTrajectories) { std::lock_guard resetLock(resetMutex_); mpc_.reset(); - mpc_.getSolverPtr()->getReferenceManager().setTargetTrajectories(std::move(initTargetTrajectories)); + mpc_.getSolverPtr()->getReferenceManager().setTargetTrajectories( + std::move(initTargetTrajectories)); mpcTimer_.reset(); resetRequestedEver_ = true; terminateThread_ = false; @@ -74,90 +76,101 @@ void MPC_ROS_Interface::resetMpcNode(TargetTrajectories&& initTargetTrajectories /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -bool MPC_ROS_Interface::resetMpcCallback(ocs2_msgs::reset::Request& req, ocs2_msgs::reset::Response& res) { - if (static_cast(req.reset)) { - auto targetTrajectories = ros_msg_conversions::readTargetTrajectoriesMsg(req.targetTrajectories); +bool MPC_ROS_Interface::resetMpcCallback( + const std::shared_ptr req, + std::shared_ptr res) { + if (static_cast(req->reset)) { + auto targetTrajectories = ros_msg_conversions::readTargetTrajectoriesMsg( + req->target_trajectories); resetMpcNode(std::move(targetTrajectories)); - res.done = static_cast(true); + res->done = static_cast(true); std::cerr << "\n#####################################################" << "\n#####################################################" << "\n################# MPC is reset. ###################" << "\n#####################################################" << "\n#####################################################\n"; - return true; - + return true; } else { - ROS_WARN_STREAM("[MPC_ROS_Interface] Reset request failed!"); - return false; + RCLCPP_WARN_STREAM(LOGGER, "[MPC_ROS_Interface] Reset request failed!"); + return false; } } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::mpc_flattened_controller MPC_ROS_Interface::createMpcPolicyMsg(const PrimalSolution& primalSolution, - const CommandData& commandData, - const PerformanceIndex& performanceIndices) { - ocs2_msgs::mpc_flattened_controller mpcPolicyMsg; +ocs2_msgs::msg::MpcFlattenedController MPC_ROS_Interface::createMpcPolicyMsg( + const PrimalSolution& primalSolution, const CommandData& commandData, + const PerformanceIndex& performanceIndices) { + ocs2_msgs::msg::MpcFlattenedController mpcPolicyMsg; - mpcPolicyMsg.initObservation = ros_msg_conversions::createObservationMsg(commandData.mpcInitObservation_); - mpcPolicyMsg.planTargetTrajectories = ros_msg_conversions::createTargetTrajectoriesMsg(commandData.mpcTargetTrajectories_); - mpcPolicyMsg.modeSchedule = ros_msg_conversions::createModeScheduleMsg(primalSolution.modeSchedule_); - mpcPolicyMsg.performanceIndices = - ros_msg_conversions::createPerformanceIndicesMsg(commandData.mpcInitObservation_.time, performanceIndices); + mpcPolicyMsg.init_observation = ros_msg_conversions::createObservationMsg( + commandData.mpcInitObservation_); + mpcPolicyMsg.plan_target_trajectories = + ros_msg_conversions::createTargetTrajectoriesMsg( + commandData.mpcTargetTrajectories_); + mpcPolicyMsg.mode_schedule = + ros_msg_conversions::createModeScheduleMsg(primalSolution.modeSchedule_); + mpcPolicyMsg.performance_indices = + ros_msg_conversions::createPerformanceIndicesMsg( + commandData.mpcInitObservation_.time, performanceIndices); switch (primalSolution.controllerPtr_->getType()) { case ControllerType::FEEDFORWARD: - mpcPolicyMsg.controllerType = ocs2_msgs::mpc_flattened_controller::CONTROLLER_FEEDFORWARD; + mpcPolicyMsg.controller_type = + ocs2_msgs::msg::MpcFlattenedController::CONTROLLER_FEEDFORWARD; break; case ControllerType::LINEAR: - mpcPolicyMsg.controllerType = ocs2_msgs::mpc_flattened_controller::CONTROLLER_LINEAR; + mpcPolicyMsg.controller_type = + ocs2_msgs::msg::MpcFlattenedController::CONTROLLER_LINEAR; break; default: - throw std::runtime_error("MPC_ROS_Interface::createMpcPolicyMsg: Unknown ControllerType"); + throw std::runtime_error( + "MPC_ROS_Interface::createMpcPolicyMsg: Unknown ControllerType"); } // maximum length of the message const size_t N = primalSolution.timeTrajectory_.size(); - mpcPolicyMsg.timeTrajectory.clear(); - mpcPolicyMsg.timeTrajectory.reserve(N); - mpcPolicyMsg.stateTrajectory.clear(); - mpcPolicyMsg.stateTrajectory.reserve(N); + mpcPolicyMsg.time_trajectory.clear(); + mpcPolicyMsg.time_trajectory.reserve(N); + mpcPolicyMsg.state_trajectory.clear(); + mpcPolicyMsg.state_trajectory.reserve(N); mpcPolicyMsg.data.clear(); mpcPolicyMsg.data.reserve(N); - mpcPolicyMsg.postEventIndices.clear(); - mpcPolicyMsg.postEventIndices.reserve(primalSolution.postEventIndices_.size()); + mpcPolicyMsg.post_event_indices.clear(); + mpcPolicyMsg.post_event_indices.reserve( + primalSolution.postEventIndices_.size()); // time for (auto t : primalSolution.timeTrajectory_) { - mpcPolicyMsg.timeTrajectory.emplace_back(t); + mpcPolicyMsg.time_trajectory.emplace_back(t); } // post-event indices for (auto ind : primalSolution.postEventIndices_) { - mpcPolicyMsg.postEventIndices.emplace_back(static_cast(ind)); + mpcPolicyMsg.post_event_indices.emplace_back(static_cast(ind)); } // state for (size_t k = 0; k < N; k++) { - ocs2_msgs::mpc_state mpcState; + ocs2_msgs::msg::MpcState mpcState; mpcState.value.resize(primalSolution.stateTrajectory_[k].rows()); for (size_t j = 0; j < primalSolution.stateTrajectory_[k].rows(); j++) { mpcState.value[j] = primalSolution.stateTrajectory_[k](j); } - mpcPolicyMsg.stateTrajectory.emplace_back(mpcState); + mpcPolicyMsg.state_trajectory.emplace_back(mpcState); } // end of k loop // input for (size_t k = 0; k < N; k++) { - ocs2_msgs::mpc_input mpcInput; + ocs2_msgs::msg::MpcInput mpcInput; mpcInput.value.resize(primalSolution.inputTrajectory_[k].rows()); for (size_t j = 0; j < primalSolution.inputTrajectory_[k].rows(); j++) { mpcInput.value[j] = primalSolution.inputTrajectory_[k](j); } - mpcPolicyMsg.inputTrajectory.emplace_back(mpcInput); + mpcPolicyMsg.input_trajectory.emplace_back(mpcInput); } // end of k loop // controller @@ -165,14 +178,15 @@ ocs2_msgs::mpc_flattened_controller MPC_ROS_Interface::createMpcPolicyMsg(const std::vector*> policyMsgDataPointers; policyMsgDataPointers.reserve(N); for (auto t : primalSolution.timeTrajectory_) { - mpcPolicyMsg.data.emplace_back(ocs2_msgs::controller_data()); + mpcPolicyMsg.data.emplace_back(ocs2_msgs::msg::ControllerData()); policyMsgDataPointers.push_back(&mpcPolicyMsg.data.back().data); timeTrajectoryTruncated.push_back(t); } // end of k loop // serialize controller into data buffer - primalSolution.controllerPtr_->flatten(timeTrajectoryTruncated, policyMsgDataPointers); + primalSolution.controllerPtr_->flatten(timeTrajectoryTruncated, + policyMsgDataPointers); return mpcPolicyMsg; } @@ -197,11 +211,12 @@ void MPC_ROS_Interface::publisherWorker() { publisherPerformanceIndicesPtr_.swap(bufferPerformanceIndicesPtr_); } - ocs2_msgs::mpc_flattened_controller mpcPolicyMsg = - createMpcPolicyMsg(*publisherPrimalSolutionPtr_, *publisherCommandPtr_, *publisherPerformanceIndicesPtr_); + ocs2_msgs::msg::MpcFlattenedController mpcPolicyMsg = + createMpcPolicyMsg(*publisherPrimalSolutionPtr_, *publisherCommandPtr_, + *publisherPerformanceIndicesPtr_); // publish the message - mpcPolicyPublisher_.publish(mpcPolicyMsg); + mpcPolicyPublisher_->publish(mpcPolicyMsg); readyToPublish_ = false; lk.unlock(); @@ -212,20 +227,24 @@ void MPC_ROS_Interface::publisherWorker() { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MPC_ROS_Interface::copyToBuffer(const SystemObservation& mpcInitObservation) { +void MPC_ROS_Interface::copyToBuffer( + const SystemObservation& mpcInitObservation) { // buffer policy mutex std::lock_guard policyBufferLock(bufferMutex_); // get solution - scalar_t finalTime = mpcInitObservation.time + mpc_.settings().solutionTimeWindow_; + scalar_t finalTime = + mpcInitObservation.time + mpc_.settings().solutionTimeWindow_; if (mpc_.settings().solutionTimeWindow_ < 0) { finalTime = mpc_.getSolverPtr()->getFinalTime(); } - mpc_.getSolverPtr()->getPrimalSolution(finalTime, bufferPrimalSolutionPtr_.get()); + mpc_.getSolverPtr()->getPrimalSolution(finalTime, + bufferPrimalSolutionPtr_.get()); // command bufferCommandPtr_->mpcInitObservation_ = mpcInitObservation; - bufferCommandPtr_->mpcTargetTrajectories_ = mpc_.getSolverPtr()->getReferenceManager().getTargetTrajectories(); + bufferCommandPtr_->mpcTargetTrajectories_ = + mpc_.getSolverPtr()->getReferenceManager().getTargetTrajectories(); // performance indices *bufferPerformanceIndicesPtr_ = mpc_.getSolverPtr()->getPerformanceIndeces(); @@ -234,11 +253,14 @@ void MPC_ROS_Interface::copyToBuffer(const SystemObservation& mpcInitObservation /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MPC_ROS_Interface::mpcObservationCallback(const ocs2_msgs::mpc_observation::ConstPtr& msg) { +void MPC_ROS_Interface::mpcObservationCallback( + const ocs2_msgs::msg::MpcObservation::ConstSharedPtr& msg) { std::lock_guard resetLock(resetMutex_); if (!resetRequestedEver_.load()) { - ROS_WARN_STREAM("MPC should be reset first. Either call MPC_ROS_Interface::reset() or use the reset service."); + RCLCPP_WARN_STREAM(LOGGER, + "MPC should be reset first. Either call " + "MPC_ROS_Interface::reset() or use the reset service."); return; } @@ -249,7 +271,8 @@ void MPC_ROS_Interface::mpcObservationCallback(const ocs2_msgs::mpc_observation: mpcTimer_.startTimer(); // run MPC - bool controllerIsUpdated = mpc_.run(currentObservation.time, currentObservation.state); + bool controllerIsUpdated = + mpc_.run(currentObservation.time, currentObservation.state); if (!controllerIsUpdated) { return; } @@ -264,16 +287,22 @@ void MPC_ROS_Interface::mpcObservationCallback(const ocs2_msgs::mpc_observation: timeWindow = mpc_.getSolverPtr()->getFinalTime() - currentObservation.time; } if (timeWindow < 2.0 * mpcTimer_.getAverageInMilliseconds() * 1e-3) { - std::cerr << "WARNING: The solution time window might be shorter than the MPC delay!\n"; + std::cerr << "WARNING: The solution time window might be shorter than the " + "MPC delay!\n"; } // display if (mpc_.settings().debugPrint_) { std::cerr << '\n'; std::cerr << "\n### MPC_ROS Benchmarking"; - std::cerr << "\n### Maximum : " << mpcTimer_.getMaxIntervalInMilliseconds() << "[ms] in interval " << mpcTimer_.getMaxIntervalIndex() << "."; - std::cerr << "\n### Average : " << mpcTimer_.getAverageInMilliseconds() << "[ms]."; - std::cerr << "\n### Latest : " << mpcTimer_.getLastIntervalInMilliseconds() << "[ms]." << std::endl; + std::cerr << "\n### Maximum : " + << mpcTimer_.getMaxIntervalInMilliseconds() << "[ms]. in interval " + << mpcTimer_.getMaxIntervalIndex() << "."; + std::cerr << "\n### Average : " << mpcTimer_.getAverageInMilliseconds() + << "[ms]."; + std::cerr << "\n### Latest : " + << mpcTimer_.getLastIntervalInMilliseconds() << "[ms]." + << std::endl; } #ifdef PUBLISH_THREAD @@ -283,8 +312,9 @@ void MPC_ROS_Interface::mpcObservationCallback(const ocs2_msgs::mpc_observation: msgReady_.notify_one(); #else - ocs2_msgs::mpc_flattened_controller mpcPolicyMsg = - createMpcPolicyMsg(*bufferPrimalSolutionPtr_, *bufferCommandPtr_, *bufferPerformanceIndicesPtr_); + ocs2_msgs::msg::MpcFlattenedController mpcPolicyMsg = + createMpcPolicyMsg(*bufferPrimalSolutionPtr_, *bufferCommandPtr_, + *bufferPerformanceIndicesPtr_); mpcPolicyPublisher_.publish(mpcPolicyMsg); #endif } @@ -294,7 +324,7 @@ void MPC_ROS_Interface::mpcObservationCallback(const ocs2_msgs::mpc_observation: /******************************************************************************************************/ void MPC_ROS_Interface::shutdownNode() { #ifdef PUBLISH_THREAD - ROS_INFO_STREAM("Shutting down workers ..."); + RCLCPP_INFO(LOGGER, "Shutting down workers ..."); std::unique_lock lk(publisherMutex_); terminateThread_ = true; @@ -306,46 +336,54 @@ void MPC_ROS_Interface::shutdownNode() { publisherWorker_.join(); } - ROS_INFO_STREAM("All workers are shut down."); + RCLCPP_INFO(LOGGER, "All workers are shut down."); #endif - - // shutdown publishers - mpcPolicyPublisher_.shutdown(); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ void MPC_ROS_Interface::spin() { - ROS_INFO_STREAM("Start spinning now ..."); + RCLCPP_INFO(LOGGER, "Start spinning now ..."); // Equivalent to ros::spin() + check if master is alive - while (::ros::ok() && ::ros::master::check()) { - ::ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(0.1)); + while (rclcpp::ok()) { + rclcpp::spin_some(node_); } } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MPC_ROS_Interface::launchNodes(ros::NodeHandle& nodeHandle) { - ROS_INFO_STREAM("MPC node is setting up ..."); +void MPC_ROS_Interface::launchNodes(const rclcpp::Node::SharedPtr& node) { + RCLCPP_INFO(LOGGER, "MPC node is setting up ..."); + node_ = node; // Observation subscriber - mpcObservationSubscriber_ = nodeHandle.subscribe(topicPrefix_ + "_mpc_observation", 1, &MPC_ROS_Interface::mpcObservationCallback, this, - ::ros::TransportHints().tcpNoDelay()); + mpcObservationSubscriber_ = + node_->create_subscription( + topicPrefix_ + "_mpc_observation", 1, + std::bind(&MPC_ROS_Interface::mpcObservationCallback, this, + std::placeholders::_1)); // MPC publisher - mpcPolicyPublisher_ = nodeHandle.advertise(topicPrefix_ + "_mpc_policy", 1, true); + mpcPolicyPublisher_ = + node_->create_publisher( + topicPrefix_ + "_mpc_policy", 1); // MPC reset service server - mpcResetServiceServer_ = nodeHandle.advertiseService(topicPrefix_ + "_mpc_reset", &MPC_ROS_Interface::resetMpcCallback, this); + mpcResetServiceServer_ = node_->create_service( + topicPrefix_ + "_mpc_reset", + [this](const std::shared_ptr& request, + const std::shared_ptr& response) { + return resetMpcCallback(request, response); + }); // display #ifdef PUBLISH_THREAD - ROS_INFO_STREAM("Publishing SLQ-MPC messages on a separate thread."); + RCLCPP_INFO(LOGGER, "Publishing SLQ-MPC messages on a separate thread."); #endif - ROS_INFO_STREAM("MPC node is ready."); + RCLCPP_INFO(LOGGER, "MPC node is ready."); // spin spin(); diff --git a/ocs2_ros_interfaces/src/mrt/LoopshapingDummyObserver.cpp b/ocs2_ros_interfaces/src/mrt/LoopshapingDummyObserver.cpp index 79e63676b..0f44e66eb 100644 --- a/ocs2_ros_interfaces/src/mrt/LoopshapingDummyObserver.cpp +++ b/ocs2_ros_interfaces/src/mrt/LoopshapingDummyObserver.cpp @@ -34,15 +34,20 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { -LoopshapingDummyObserver::LoopshapingDummyObserver(std::shared_ptr loopshapingDefinitionPtr, - std::vector> observersPtrArray) - : loopshapingDefinitionPtr_(std::move(loopshapingDefinitionPtr)), observersPtrArray_(std::move(observersPtrArray)) {} - -void LoopshapingDummyObserver::update(const SystemObservation& observation, const PrimalSolution& primalSolution, +LoopshapingDummyObserver::LoopshapingDummyObserver( + std::shared_ptr loopshapingDefinitionPtr, + std::vector> observersPtrArray) + : loopshapingDefinitionPtr_(std::move(loopshapingDefinitionPtr)), + observersPtrArray_(std::move(observersPtrArray)) {} + +void LoopshapingDummyObserver::update(const SystemObservation& observation, + const PrimalSolution& primalSolution, const CommandData& command) { if (!observersPtrArray_.empty()) { - const auto systemObservation = loopshapingToSystemObservation(observation, *loopshapingDefinitionPtr_); - const auto systemPrimalSolution = loopshapingToSystemPrimalSolution(primalSolution, *loopshapingDefinitionPtr_); + const auto systemObservation = + loopshapingToSystemObservation(observation, *loopshapingDefinitionPtr_); + const auto systemPrimalSolution = loopshapingToSystemPrimalSolution( + primalSolution, *loopshapingDefinitionPtr_); for (auto& observer : observersPtrArray_) { observer->update(systemObservation, systemPrimalSolution, command); diff --git a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp index bcbc29865..a9c760c10 100644 --- a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp +++ b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp @@ -32,36 +32,46 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { +const rclcpp::Logger LOGGER = rclcpp::get_logger("MRT_ROS_Dummy_Loop"); + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MRT_ROS_Dummy_Loop::MRT_ROS_Dummy_Loop(MRT_ROS_Interface& mrt, scalar_t mrtDesiredFrequency, scalar_t mpcDesiredFrequency) - : mrt_(mrt), mrtDesiredFrequency_(mrtDesiredFrequency), mpcDesiredFrequency_(mpcDesiredFrequency) { +MRT_ROS_Dummy_Loop::MRT_ROS_Dummy_Loop(MRT_ROS_Interface& mrt, + scalar_t mrtDesiredFrequency, + scalar_t mpcDesiredFrequency) + : mrt_(mrt), + mrtDesiredFrequency_(mrtDesiredFrequency), + mpcDesiredFrequency_(mpcDesiredFrequency) { if (mrtDesiredFrequency_ < 0) { throw std::runtime_error("MRT loop frequency should be a positive number."); } if (mpcDesiredFrequency_ > 0) { - ROS_WARN_STREAM("MPC loop is not realtime! For realtime setting, set mpcDesiredFrequency to any negative number."); + RCLCPP_WARN_STREAM(LOGGER, + "MPC loop is not realtime! For realtime setting, set " + "mpcDesiredFrequency to any negative number."); } } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MRT_ROS_Dummy_Loop::run(const SystemObservation& initObservation, const TargetTrajectories& initTargetTrajectories) { - ROS_INFO_STREAM("Waiting for the initial policy ..."); +void MRT_ROS_Dummy_Loop::run(const SystemObservation& initObservation, + const TargetTrajectories& initTargetTrajectories) { + RCLCPP_INFO_STREAM(LOGGER, "Waiting for the initial policy ..."); + rclcpp::Rate loop_rate(mrtDesiredFrequency_); // Reset MPC node mrt_.resetMpcNode(initTargetTrajectories); // Wait for the initial policy - while (!mrt_.initialPolicyReceived() && ros::ok() && ros::master::check()) { + while (!mrt_.initialPolicyReceived() && rclcpp::ok()) { mrt_.spinMRT(); mrt_.setCurrentObservation(initObservation); - ros::Rate(mrtDesiredFrequency_).sleep(); + loop_rate.sleep(); } - ROS_INFO_STREAM("Initial policy has been received."); + RCLCPP_INFO_STREAM(LOGGER, "Initial policy has been received."); // Pick simulation loop mode if (mpcDesiredFrequency_ > 0.0) { @@ -74,23 +84,31 @@ void MRT_ROS_Dummy_Loop::run(const SystemObservation& initObservation, const Tar /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MRT_ROS_Dummy_Loop::synchronizedDummyLoop(const SystemObservation& initObservation, const TargetTrajectories& initTargetTrajectories) { +void MRT_ROS_Dummy_Loop::synchronizedDummyLoop( + const SystemObservation& initObservation, + const TargetTrajectories& initTargetTrajectories) { // Determine the ratio between MPC updates and simulation steps. - const auto mpcUpdateRatio = std::max(static_cast(mrtDesiredFrequency_ / mpcDesiredFrequency_), size_t(1)); + const auto mpcUpdateRatio = + std::max(static_cast(mrtDesiredFrequency_ / mpcDesiredFrequency_), + size_t(1)); // Loop variables size_t loopCounter = 0; SystemObservation currentObservation = initObservation; // Helper function to check if policy is updated and starts at the given time. - // Due to ROS message conversion delay and very fast MPC loop, we might get an old policy instead of the latest one. + // Due to ROS message conversion delay and very fast MPC loop, we might get an + // old policy instead of the latest one. const auto policyUpdatedForTime = [this](scalar_t time) { - constexpr scalar_t tol = 0.1; // policy must start within this fraction of dt - return mrt_.updatePolicy() && std::abs(mrt_.getPolicy().timeTrajectory_.front() - time) < (tol / mpcDesiredFrequency_); + constexpr scalar_t tol = + 0.1; // policy must start within this fraction of dt + return mrt_.updatePolicy() && + std::abs(mrt_.getPolicy().timeTrajectory_.front() - time) < + (tol / mpcDesiredFrequency_); }; - ros::Rate simRate(mrtDesiredFrequency_); - while (ros::ok() && ros::master::check()) { + rclcpp::Rate simRate(mrtDesiredFrequency_); + while (rclcpp::ok()) { std::cout << "### Current time " << currentObservation.time << "\n"; // Trigger MRT callbacks @@ -99,10 +117,11 @@ void MRT_ROS_Dummy_Loop::synchronizedDummyLoop(const SystemObservation& initObse // Update the MPC policy if it is time to do so if (loopCounter % mpcUpdateRatio == 0) { // Wait for the policy to be updated - while (!policyUpdatedForTime(currentObservation.time) && ros::ok() && ros::master::check()) { + while (!policyUpdatedForTime(currentObservation.time) && rclcpp::ok()) { mrt_.spinMRT(); } - std::cout << "<<< New MPC policy starting at " << mrt_.getPolicy().timeTrajectory_.front() << "\n"; + std::cout << "<<< New MPC policy starting at " + << mrt_.getPolicy().timeTrajectory_.front() << "\n"; } // measure the delay in running MRT @@ -120,7 +139,8 @@ void MRT_ROS_Dummy_Loop::synchronizedDummyLoop(const SystemObservation& initObse // Publish observation if at the next step we want a new policy if ((loopCounter + 1) % mpcUpdateRatio == 0) { mrt_.setCurrentObservation(currentObservation); - std::cout << ">>> Observation is published at " << currentObservation.time << "\n"; + std::cout << ">>> Observation is published at " << currentObservation.time + << "\n"; } // Update observers @@ -136,7 +156,7 @@ void MRT_ROS_Dummy_Loop::synchronizedDummyLoop(const SystemObservation& initObse std::cerr << "\n### Latest : " << mrtTimer_.getLastIntervalInMilliseconds() << "[ms]." << std::endl; ++loopCounter; - ros::spinOnce(); + mrt_.spinMRT(); simRate.sleep(); } } @@ -144,12 +164,14 @@ void MRT_ROS_Dummy_Loop::synchronizedDummyLoop(const SystemObservation& initObse /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MRT_ROS_Dummy_Loop::realtimeDummyLoop(const SystemObservation& initObservation, const TargetTrajectories& initTargetTrajectories) { +void MRT_ROS_Dummy_Loop::realtimeDummyLoop( + const SystemObservation& initObservation, + const TargetTrajectories& initTargetTrajectories) { // Loop variables SystemObservation currentObservation = initObservation; - ros::Rate simRate(mrtDesiredFrequency_); - while (ros::ok() && ros::master::check()) { + rclcpp::Rate simRate(mrtDesiredFrequency_); + while (rclcpp::ok()) { std::cout << "### Current time " << currentObservation.time << "\n"; // Trigger MRT callbacks @@ -157,7 +179,8 @@ void MRT_ROS_Dummy_Loop::realtimeDummyLoop(const SystemObservation& initObservat // Update the policy if a new on was received if (mrt_.updatePolicy()) { - std::cout << "<<< New MPC policy starting at " << mrt_.getPolicy().timeTrajectory_.front() << "\n"; + std::cout << "<<< New MPC policy starting at " + << mrt_.getPolicy().timeTrajectory_.front() << "\n"; } // Forward simulation @@ -174,7 +197,7 @@ void MRT_ROS_Dummy_Loop::realtimeDummyLoop(const SystemObservation& initObservat observer->update(currentObservation, mrt_.getPolicy(), mrt_.getCommand()); } - ros::spinOnce(); + mrt_.spinMRT(); simRate.sleep(); } } @@ -182,16 +205,21 @@ void MRT_ROS_Dummy_Loop::realtimeDummyLoop(const SystemObservation& initObservat /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -SystemObservation MRT_ROS_Dummy_Loop::forwardSimulation(const SystemObservation& currentObservation) { +SystemObservation MRT_ROS_Dummy_Loop::forwardSimulation( + const SystemObservation& currentObservation) { const scalar_t dt = 1.0 / mrtDesiredFrequency_; SystemObservation nextObservation; nextObservation.time = currentObservation.time + dt; - if (mrt_.isRolloutSet()) { // If available, use the provided rollout as to integrate the dynamics. - mrt_.rolloutPolicy(currentObservation.time, currentObservation.state, dt, nextObservation.state, nextObservation.input, + if (mrt_.isRolloutSet()) { // If available, use the provided rollout as to + // integrate the dynamics. + mrt_.rolloutPolicy(currentObservation.time, currentObservation.state, dt, + nextObservation.state, nextObservation.input, nextObservation.mode); - } else { // Otherwise, we fake integration by interpolating the current MPC policy at t+dt - mrt_.evaluatePolicy(currentObservation.time + dt, currentObservation.state, nextObservation.state, nextObservation.input, + } else { // Otherwise, we fake integration by interpolating the current MPC + // policy at t+dt + mrt_.evaluatePolicy(currentObservation.time + dt, currentObservation.state, + nextObservation.state, nextObservation.input, nextObservation.mode); } diff --git a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp index 9d013071d..73f959434 100644 --- a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp +++ b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp @@ -34,56 +34,64 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { +const rclcpp::Logger LOGGER = rclcpp::get_logger("MRT_ROS_Interface"); + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MRT_ROS_Interface::MRT_ROS_Interface(std::string topicPrefix, ros::TransportHints mrtTransportHints) - : topicPrefix_(std::move(topicPrefix)), mrtTransportHints_(mrtTransportHints) { +MRT_ROS_Interface::MRT_ROS_Interface(std::string topicPrefix) + : topicPrefix_(std::move(topicPrefix)) { // Start thread for publishing #ifdef PUBLISH_THREAD // Close old thread if it is already running shutdownPublisher(); terminateThread_ = false; readyToPublish_ = false; - publisherWorker_ = std::thread(&MRT_ROS_Interface::publisherWorkerThread, this); + publisherWorker_ = + std::thread(&MRT_ROS_Interface::publisherWorkerThread, this); #endif } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MRT_ROS_Interface::~MRT_ROS_Interface() { - shutdownNodes(); -} +MRT_ROS_Interface::~MRT_ROS_Interface() { shutdownNodes(); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MRT_ROS_Interface::resetMpcNode(const TargetTrajectories& initTargetTrajectories) { +void MRT_ROS_Interface::resetMpcNode( + const TargetTrajectories& initTargetTrajectories) { this->reset(); - ocs2_msgs::reset resetSrv; - resetSrv.request.reset = static_cast(true); - resetSrv.request.targetTrajectories = ros_msg_conversions::createTargetTrajectoriesMsg(initTargetTrajectories); + auto resetSrvRequest = std::make_shared(); + ; + resetSrvRequest->reset = static_cast(true); + resetSrvRequest->target_trajectories = + ros_msg_conversions::createTargetTrajectoriesMsg(initTargetTrajectories); - while (!mpcResetServiceClient_.waitForExistence(ros::Duration(5.0)) && ::ros::ok() && ::ros::master::check()) { - ROS_ERROR_STREAM("Failed to call service to reset MPC, retrying..."); + while (!mpcResetServiceClient_->wait_for_service(std::chrono::seconds(5)) && + rclcpp::ok()) { + RCLCPP_ERROR_STREAM(LOGGER, + "Failed to call service to reset MPC, retrying..."); } - mpcResetServiceClient_.call(resetSrv); - ROS_INFO_STREAM("MPC node has been reset."); + mpcResetServiceClient_->async_send_request(resetSrvRequest); + RCLCPP_INFO_STREAM(LOGGER, "MPC node has been reset."); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MRT_ROS_Interface::setCurrentObservation(const SystemObservation& currentObservation) { +void MRT_ROS_Interface::setCurrentObservation( + const SystemObservation& currentObservation) { #ifdef PUBLISH_THREAD std::unique_lock lk(publisherMutex_); #endif // create the message - mpcObservationMsg_ = ros_msg_conversions::createObservationMsg(currentObservation); + mpcObservationMsg_ = + ros_msg_conversions::createObservationMsg(currentObservation); // publish the current observation #ifdef PUBLISH_THREAD @@ -115,33 +123,43 @@ void MRT_ROS_Interface::publisherWorkerThread() { lk.unlock(); msgReady_.notify_one(); - mpcObservationPublisher_.publish(mpcObservationMsgBuffer_); + mpcObservationPublisher_->publish(mpcObservationMsgBuffer_); } } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MRT_ROS_Interface::readPolicyMsg(const ocs2_msgs::mpc_flattened_controller& msg, CommandData& commandData, - PrimalSolution& primalSolution, PerformanceIndex& performanceIndices) { - commandData.mpcInitObservation_ = ros_msg_conversions::readObservationMsg(msg.initObservation); - commandData.mpcTargetTrajectories_ = ros_msg_conversions::readTargetTrajectoriesMsg(msg.planTargetTrajectories); - performanceIndices = ros_msg_conversions::readPerformanceIndicesMsg(msg.performanceIndices); +void MRT_ROS_Interface::readPolicyMsg( + const ocs2_msgs::msg::MpcFlattenedController& msg, CommandData& commandData, + PrimalSolution& primalSolution, PerformanceIndex& performanceIndices) { + commandData.mpcInitObservation_ = + ros_msg_conversions::readObservationMsg(msg.init_observation); + commandData.mpcTargetTrajectories_ = + ros_msg_conversions::readTargetTrajectoriesMsg( + msg.plan_target_trajectories); + performanceIndices = + ros_msg_conversions::readPerformanceIndicesMsg(msg.performance_indices); - const size_t N = msg.timeTrajectory.size(); + const size_t N = msg.time_trajectory.size(); if (N == 0) { - throw std::runtime_error("[MRT_ROS_Interface::readPolicyMsg] controller message is empty!"); + throw std::runtime_error( + "[MRT_ROS_Interface::readPolicyMsg] controller message is empty!"); } - if (msg.stateTrajectory.size() != N && msg.inputTrajectory.size() != N) { - throw std::runtime_error("[MRT_ROS_Interface::readPolicyMsg] state and input trajectories must have same length!"); + if (msg.state_trajectory.size() != N && msg.input_trajectory.size() != N) { + throw std::runtime_error( + "[MRT_ROS_Interface::readPolicyMsg] state and input trajectories must " + "have same length!"); } if (msg.data.size() != N) { - throw std::runtime_error("[MRT_ROS_Interface::readPolicyMsg] Data has the wrong length!"); + throw std::runtime_error( + "[MRT_ROS_Interface::readPolicyMsg] Data has the wrong length!"); } primalSolution.clear(); - primalSolution.modeSchedule_ = ros_msg_conversions::readModeScheduleMsg(msg.modeSchedule); + primalSolution.modeSchedule_ = + ros_msg_conversions::readModeScheduleMsg(msg.mode_schedule); size_array_t stateDim(N); size_array_t inputDim(N); @@ -149,17 +167,21 @@ void MRT_ROS_Interface::readPolicyMsg(const ocs2_msgs::mpc_flattened_controller& primalSolution.stateTrajectory_.reserve(N); primalSolution.inputTrajectory_.reserve(N); for (size_t i = 0; i < N; i++) { - stateDim[i] = msg.stateTrajectory[i].value.size(); - inputDim[i] = msg.inputTrajectory[i].value.size(); - primalSolution.timeTrajectory_.emplace_back(msg.timeTrajectory[i]); + stateDim[i] = msg.state_trajectory[i].value.size(); + inputDim[i] = msg.input_trajectory[i].value.size(); + primalSolution.timeTrajectory_.emplace_back(msg.time_trajectory[i]); primalSolution.stateTrajectory_.emplace_back( - Eigen::Map(msg.stateTrajectory[i].value.data(), stateDim[i]).cast()); + Eigen::Map(msg.state_trajectory[i].value.data(), + stateDim[i]) + .cast()); primalSolution.inputTrajectory_.emplace_back( - Eigen::Map(msg.inputTrajectory[i].value.data(), inputDim[i]).cast()); + Eigen::Map(msg.input_trajectory[i].value.data(), + inputDim[i]) + .cast()); } - primalSolution.postEventIndices_.reserve(msg.postEventIndices.size()); - for (auto ind : msg.postEventIndices) { + primalSolution.postEventIndices_.reserve(msg.post_event_indices.size()); + for (auto ind : msg.post_event_indices) { primalSolution.postEventIndices_.emplace_back(static_cast(ind)); } @@ -169,33 +191,41 @@ void MRT_ROS_Interface::readPolicyMsg(const ocs2_msgs::mpc_flattened_controller& } // instantiate the correct controller - switch (msg.controllerType) { - case ocs2_msgs::mpc_flattened_controller::CONTROLLER_FEEDFORWARD: { - auto controller = FeedforwardController::unFlatten(primalSolution.timeTrajectory_, controllerDataPtrArray); - primalSolution.controllerPtr_.reset(new FeedforwardController(std::move(controller))); + switch (msg.controller_type) { + case ocs2_msgs::msg::MpcFlattenedController::CONTROLLER_FEEDFORWARD: { + auto controller = FeedforwardController::unFlatten( + primalSolution.timeTrajectory_, controllerDataPtrArray); + primalSolution.controllerPtr_.reset( + new FeedforwardController(std::move(controller))); break; } - case ocs2_msgs::mpc_flattened_controller::CONTROLLER_LINEAR: { - auto controller = LinearController::unFlatten(stateDim, inputDim, primalSolution.timeTrajectory_, controllerDataPtrArray); - primalSolution.controllerPtr_.reset(new LinearController(std::move(controller))); + case ocs2_msgs::msg::MpcFlattenedController::CONTROLLER_LINEAR: { + auto controller = LinearController::unFlatten( + stateDim, inputDim, primalSolution.timeTrajectory_, + controllerDataPtrArray); + primalSolution.controllerPtr_.reset( + new LinearController(std::move(controller))); break; } default: - throw std::runtime_error("[MRT_ROS_Interface::readPolicyMsg] Unknown controllerType!"); + throw std::runtime_error( + "[MRT_ROS_Interface::readPolicyMsg] Unknown controllerType!"); } } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MRT_ROS_Interface::mpcPolicyCallback(const ocs2_msgs::mpc_flattened_controller::ConstPtr& msg) { +void MRT_ROS_Interface::mpcPolicyCallback( + const ocs2_msgs::msg::MpcFlattenedController::ConstSharedPtr& msg) { // read new policy and command from msg auto commandPtr = std::make_unique(); auto primalSolutionPtr = std::make_unique(); auto performanceIndicesPtr = std::make_unique(); readPolicyMsg(*msg, *commandPtr, *primalSolutionPtr, *performanceIndicesPtr); - this->moveToBuffer(std::move(commandPtr), std::move(primalSolutionPtr), std::move(performanceIndicesPtr)); + this->moveToBuffer(std::move(commandPtr), std::move(primalSolutionPtr), + std::move(performanceIndicesPtr)); } /******************************************************************************************************/ @@ -203,19 +233,12 @@ void MRT_ROS_Interface::mpcPolicyCallback(const ocs2_msgs::mpc_flattened_control /******************************************************************************************************/ void MRT_ROS_Interface::shutdownNodes() { #ifdef PUBLISH_THREAD - ROS_INFO_STREAM("Shutting down workers ..."); + RCLCPP_INFO_STREAM(LOGGER, "Shutting down workers ..."); shutdownPublisher(); - ROS_INFO_STREAM("All workers are shut down."); + RCLCPP_INFO_STREAM(LOGGER, "All workers are shut down."); #endif - - // clean up callback queue - mrtCallbackQueue_.clear(); - mpcPolicySubscriber_.shutdown(); - - // shutdown publishers - mpcObservationPublisher_.shutdown(); } /******************************************************************************************************/ @@ -237,41 +260,41 @@ void MRT_ROS_Interface::shutdownPublisher() { /******************************************************************************************************/ /******************************************************************************************************/ void MRT_ROS_Interface::spinMRT() { - mrtCallbackQueue_.callOne(); + rclcpp::spin_some(node_); }; /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MRT_ROS_Interface::launchNodes(ros::NodeHandle& nodeHandle) { +void MRT_ROS_Interface::launchNodes(const rclcpp::Node::SharedPtr& node) { this->reset(); - + node_ = node; // display - ROS_INFO_STREAM("MRT node is setting up ..."); + RCLCPP_INFO_STREAM(LOGGER, "MRT node is setting up ..."); // observation publisher - mpcObservationPublisher_ = nodeHandle.advertise(topicPrefix_ + "_mpc_observation", 1); + mpcObservationPublisher_ = + node_->create_publisher( + topicPrefix_ + "_mpc_observation", 1); // policy subscriber - auto ops = ros::SubscribeOptions::create( - topicPrefix_ + "_mpc_policy", // topic name - 1, // queue length - boost::bind(&MRT_ROS_Interface::mpcPolicyCallback, this, boost::placeholders::_1), // callback - ros::VoidConstPtr(), // tracked object - &mrtCallbackQueue_ // pointer to callback queue object - ); - ops.transport_hints = mrtTransportHints_; - mpcPolicySubscriber_ = nodeHandle.subscribe(ops); + mpcPolicySubscriber_ = + node_->create_subscription( + topicPrefix_ + "_mpc_policy", // topic name + 1, // queue length + std::bind(&MRT_ROS_Interface::mpcPolicyCallback, this, + std::placeholders::_1)); // MPC reset service client - mpcResetServiceClient_ = nodeHandle.serviceClient(topicPrefix_ + "_mpc_reset"); + mpcResetServiceClient_ = + node_->create_client(topicPrefix_ + "_mpc_reset"); // display #ifdef PUBLISH_THREAD - ROS_INFO_STREAM("Publishing MRT messages on a separate thread."); + RCLCPP_INFO_STREAM(LOGGER, "Publishing MRT messages on a separate thread."); #endif - ROS_INFO_STREAM("MRT node is ready."); + RCLCPP_INFO_STREAM(LOGGER, "MRT node is ready."); spinMRT(); } diff --git a/ocs2_ros_interfaces/src/multiplot/MultiplotRemap.cpp b/ocs2_ros_interfaces/src/multiplot/MultiplotRemap.cpp index eca6fda8d..b774607f4 100644 --- a/ocs2_ros_interfaces/src/multiplot/MultiplotRemap.cpp +++ b/ocs2_ros_interfaces/src/multiplot/MultiplotRemap.cpp @@ -27,56 +27,60 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include - -#include -#include - #include -#include -#include +#include +#include +#include + +#include namespace { -class MultiplotRemap { +class MultiplotRemap : public rclcpp::Node { public: /** Constructor */ - explicit MultiplotRemap(const std::string& mpcPolicyTopicName, ::ros::NodeHandle& nodeHandle) { + explicit MultiplotRemap(const std::string& mpcPolicyTopicName) + : Node("MultiplotRemap") { mpcPolicySubscriber_ = - nodeHandle.subscribe(mpcPolicyTopicName, 1, &MultiplotRemap::mpcPoicyCallback, this, ::ros::TransportHints().tcpNoDelay()); - mpcPerformanceIndicesPublisher_ = nodeHandle.advertise("mpc_performance_indices", 1); + this->create_subscription( + mpcPolicyTopicName, 1, + std::bind(&MultiplotRemap::mpcPoicyCallback, this, + std::placeholders::_1)); + mpcPerformanceIndicesPublisher_ = + this->create_publisher( + "mpc_performance_indices", 1); } /** Default deconstructor */ ~MultiplotRemap() = default; private: - void mpcPoicyCallback(const ocs2_msgs::mpc_flattened_controller::ConstPtr& policyMsg) { - mpcPerformanceIndicesPublisher_.publish(policyMsg->performanceIndices); + void mpcPoicyCallback( + const ocs2_msgs::msg::MpcFlattenedController::ConstSharedPtr& policyMsg) { + mpcPerformanceIndicesPublisher_->publish(policyMsg->performance_indices); } // publishers and subscribers - ::ros::Subscriber mpcPolicySubscriber_; - ::ros::Publisher mpcPerformanceIndicesPublisher_; + rclcpp::Subscription::SharedPtr + mpcPolicySubscriber_; + rclcpp::Publisher::SharedPtr + mpcPerformanceIndicesPublisher_; }; } // unnamed namespace int main(int argc, char** argv) { // mpc policy topic name - std::vector programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); + std::vector programArgs = + rclcpp::remove_ros_arguments(argc, argv); if (programArgs.size() <= 1) { throw std::runtime_error("MPC policy topic name is not specified!"); } const std::string mpcPolicyTopicName = std::string(programArgs[1]); - ::ros::init(argc, argv, "multiplot_remap"); - ::ros::NodeHandle nodeHandle; - MultiplotRemap multiplotRemap(mpcPolicyTopicName, nodeHandle); - - ::ros::spin(); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared(mpcPolicyTopicName)); return 0; } diff --git a/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp b/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp index c7e880031..3af495a4b 100644 --- a/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp +++ b/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp @@ -30,39 +30,48 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h" #include "ocs2_ros_interfaces/common/RosMsgConversions.h" - -#include +#include // MPC messages -#include -#include +#include +#include namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -RosReferenceManager::RosReferenceManager(std::string topicPrefix, std::shared_ptr referenceManagerPtr) - : ReferenceManagerDecorator(std::move(referenceManagerPtr)), topicPrefix_(std::move(topicPrefix)) {} +RosReferenceManager::RosReferenceManager( + std::string topicPrefix, + std::shared_ptr referenceManagerPtr) + : ReferenceManagerDecorator(std::move(referenceManagerPtr)), + topicPrefix_(std::move(topicPrefix)) {} /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void RosReferenceManager::subscribe(ros::NodeHandle& nodeHandle) { +void RosReferenceManager::subscribe(const rclcpp::Node::SharedPtr& node) { + node_ = node; // ModeSchedule - auto modeScheduleCallback = [this](const ocs2_msgs::mode_schedule::ConstPtr& msg) { - auto modeSchedule = ros_msg_conversions::readModeScheduleMsg(*msg); + auto modeScheduleCallback = [this](const ocs2_msgs::msg::ModeSchedule& msg) { + auto modeSchedule = ros_msg_conversions::readModeScheduleMsg(msg); referenceManagerPtr_->setModeSchedule(std::move(modeSchedule)); }; - modeScheduleSubscriber_ = nodeHandle.subscribe(topicPrefix_ + "_mode_schedule", 1, modeScheduleCallback); + modeScheduleSubscriber_ = + node_->create_subscription( + topicPrefix_ + "_mode_schedule", 1, modeScheduleCallback); // TargetTrajectories - auto targetTrajectoriesCallback = [this](const ocs2_msgs::mpc_target_trajectories::ConstPtr& msg) { - auto targetTrajectories = ros_msg_conversions::readTargetTrajectoriesMsg(*msg); - referenceManagerPtr_->setTargetTrajectories(std::move(targetTrajectories)); - }; + auto targetTrajectoriesCallback = + [this](const ocs2_msgs::msg::MpcTargetTrajectories& msg) { + auto targetTrajectories = + ros_msg_conversions::readTargetTrajectoriesMsg(msg); + referenceManagerPtr_->setTargetTrajectories( + std::move(targetTrajectories)); + }; targetTrajectoriesSubscriber_ = - nodeHandle.subscribe(topicPrefix_ + "_mpc_target", 1, targetTrajectoriesCallback); + node_->create_subscription( + topicPrefix_ + "_mpc_target", 1, targetTrajectoriesCallback); } } // namespace ocs2 diff --git a/ocs2_ros_interfaces/src/synchronized_module/SolverObserverRosCallbacks.cpp b/ocs2_ros_interfaces/src/synchronized_module/SolverObserverRosCallbacks.cpp index be5de2ad2..81bf512cb 100644 --- a/ocs2_ros_interfaces/src/synchronized_module/SolverObserverRosCallbacks.cpp +++ b/ocs2_ros_interfaces/src/synchronized_module/SolverObserverRosCallbacks.cpp @@ -38,42 +38,57 @@ namespace ros { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -SolverObserver::constraint_callback_t createConstraintCallback(::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, - const std::vector& topicNames, - CallbackInterpolationStrategy interpolationStrategy) { - using vector_ref_array_t = std::vector>; +SolverObserver::constraint_callback_t createConstraintCallback( + const rclcpp::Node::SharedPtr& node, + const scalar_array_t& observingTimePoints, + const std::vector& topicNames, + CallbackInterpolationStrategy interpolationStrategy) { + using vector_ref_array_t = + std::vector>; if (observingTimePoints.size() != topicNames.size()) { - throw std::runtime_error("[createConstraintCallback] For each observing time points, you should provide a unique topic name!"); + throw std::runtime_error( + "[createConstraintCallback] For each observing time points, you should " + "provide a unique topic name!"); } - std::vector<::ros::Publisher> constraintPublishers; + std::vector::SharedPtr> + constraintPublishers; for (const auto& name : topicNames) { - constraintPublishers.push_back(nodeHandle.advertise(name, 1, true)); + constraintPublishers.push_back( + node->create_publisher(name, 1)); } - // note that we need to copy the publishers as the local ones will go out of scope. Good news is that ROS publisher - // behaves like std::sharted_ptr ("Once all copies of a specific Publisher go out of scope, any subscriber status callbacks - // associated with that handle will stop being called.") - return [=](const scalar_array_t& timeTrajectory, const vector_ref_array_t& termConstraintArray) { + // note that we need to copy the publishers as the local ones will go out of + // scope. Good news is that ROS publisher behaves like std::sharted_ptr ("Once + // all copies of a specific Publisher go out of scope, any subscriber status + // callbacks associated with that handle will stop being called.") + return [=](const scalar_array_t& timeTrajectory, + const vector_ref_array_t& termConstraintArray) { if (!timeTrajectory.empty()) { for (size_t i = 0; i < observingTimePoints.size(); i++) { const auto t = timeTrajectory.front() + observingTimePoints[i]; - const auto indexAlpha = LinearInterpolation::timeSegment(t, timeTrajectory); + const auto indexAlpha = + LinearInterpolation::timeSegment(t, timeTrajectory); const auto constraint = [&]() -> vector_t { switch (interpolationStrategy) { case CallbackInterpolationStrategy::nearest_time: - return (indexAlpha.second > 0.5) ? termConstraintArray[indexAlpha.first].get() - : termConstraintArray[indexAlpha.first + 1].get(); + return (indexAlpha.second > 0.5) + ? termConstraintArray[indexAlpha.first].get() + : termConstraintArray[indexAlpha.first + 1].get(); case CallbackInterpolationStrategy::linear_interpolation: return LinearInterpolation::interpolate( indexAlpha, termConstraintArray, - [](const vector_ref_array_t& array, size_t t) -> const vector_t& { return array[t].get(); }); + [](const vector_ref_array_t& array, + size_t t) -> const vector_t& { return array[t].get(); }); default: - throw std::runtime_error("[createConstraintCallback] This CallbackInterpolationStrategy is not implemented!"); + throw std::runtime_error( + "[createConstraintCallback] This " + "CallbackInterpolationStrategy is not implemented!"); } }(); - constraintPublishers[i].publish(ros_msg_conversions::createConstraintMsg(t, constraint)); + constraintPublishers[i]->publish( + ros_msg_conversions::createConstraintMsg(t, constraint)); } } }; @@ -82,38 +97,54 @@ SolverObserver::constraint_callback_t createConstraintCallback(::ros::NodeHandle /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -SolverObserver::lagrangian_callback_t createLagrangianCallback(::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, - const std::vector& topicNames, - CallbackInterpolationStrategy interpolationStrategy) { +SolverObserver::lagrangian_callback_t createLagrangianCallback( + const rclcpp::Node::SharedPtr& node, + const scalar_array_t& observingTimePoints, + const std::vector& topicNames, + CallbackInterpolationStrategy interpolationStrategy) { if (observingTimePoints.size() != topicNames.size()) { - throw std::runtime_error("[createLagrangianCallback] For each observing time points, you should provide a unique topic name!"); + throw std::runtime_error( + "[createLagrangianCallback] For each observing time points, you should " + "provide a unique topic name!"); } - std::vector<::ros::Publisher> metricsPublishers; + std::vector::SharedPtr> + metricsPublishers; for (const auto& name : topicNames) { - metricsPublishers.push_back(nodeHandle.advertise(name, 1, true)); + metricsPublishers.push_back( + node->create_publisher(name, 1)); } - // note that we need to copy the publishers as the local ones will go out of scope. Good news is that ROS publisher - // behaves like std::sharted_ptr ("Once all copies of a specific Publisher go out of scope, any subscriber status callbacks - // associated with that handle will stop being called.") - return [=](const scalar_array_t& timeTrajectory, const std::vector& termMetricsArray) { + // note that we need to copy the publishers as the local ones will go out of + // scope. Good news is that ROS publisher behaves like std::sharted_ptr ("Once + // all copies of a specific Publisher go out of scope, any subscriber status + // callbacks associated with that handle will stop being called.") + return [=](const scalar_array_t& timeTrajectory, + const std::vector& termMetricsArray) { if (!timeTrajectory.empty()) { for (size_t i = 0; i < observingTimePoints.size(); i++) { const auto t = timeTrajectory.front() + observingTimePoints[i]; - const auto indexAlpha = LinearInterpolation::timeSegment(t, timeTrajectory); + const auto indexAlpha = + LinearInterpolation::timeSegment(t, timeTrajectory); const auto metrics = [&]() -> LagrangianMetrics { switch (interpolationStrategy) { case CallbackInterpolationStrategy::nearest_time: - return (indexAlpha.second > 0.5) ? static_cast(termMetricsArray[indexAlpha.first]) - : static_cast(termMetricsArray[indexAlpha.first + 1]); + return (indexAlpha.second > 0.5) + ? static_cast( + termMetricsArray[indexAlpha.first]) + : static_cast( + termMetricsArray[indexAlpha.first + 1]); case CallbackInterpolationStrategy::linear_interpolation: - return LinearInterpolation::interpolate(indexAlpha, termMetricsArray); + return LinearInterpolation::interpolate(indexAlpha, + termMetricsArray); default: - throw std::runtime_error("[createLagrangianCallback] This CallbackInterpolationStrategy is not implemented!"); + throw std::runtime_error( + "[createLagrangianCallback] This " + "CallbackInterpolationStrategy is not implemented!"); } }(); - metricsPublishers[i].publish(ros_msg_conversions::createLagrangianMetricsMsg(t, metrics)); + metricsPublishers[i]->publish( + ros_msg_conversions::createLagrangianMetricsMsg(t, metrics)); } } }; @@ -122,38 +153,54 @@ SolverObserver::lagrangian_callback_t createLagrangianCallback(::ros::NodeHandle /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -SolverObserver::multiplier_callback_t createMultiplierCallback(::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, - const std::vector& topicNames, - CallbackInterpolationStrategy interpolationStrategy) { +SolverObserver::multiplier_callback_t createMultiplierCallback( + const rclcpp::Node::SharedPtr& node, + const scalar_array_t& observingTimePoints, + const std::vector& topicNames, + CallbackInterpolationStrategy interpolationStrategy) { if (observingTimePoints.size() != topicNames.size()) { - throw std::runtime_error("[createMultiplierCallback] For each observing time points, you should provide a unique topic name!"); + throw std::runtime_error( + "[createMultiplierCallback] For each observing time points, you should " + "provide a unique topic name!"); } - std::vector<::ros::Publisher> multiplierPublishers; + std::vector::SharedPtr> + multiplierPublishers; for (const auto& name : topicNames) { - multiplierPublishers.push_back(nodeHandle.advertise(name, 1, true)); + multiplierPublishers.push_back( + node->create_publisher(name, 1)); } - // note that we need to copy the publishers as the local ones will go out of scope. Good news is that ROS publisher - // behaves like std::sharted_ptr ("Once all copies of a specific Publisher go out of scope, any subscriber status callbacks - // associated with that handle will stop being called.") - return [=](const scalar_array_t& timeTrajectory, const std::vector& termMultiplierArray) { + // note that we need to copy the publishers as the local ones will go out of + // scope. Good news is that ROS publisher behaves like std::sharted_ptr ("Once + // all copies of a specific Publisher go out of scope, any subscriber status + // callbacks associated with that handle will stop being called.") + return [=](const scalar_array_t& timeTrajectory, + const std::vector& termMultiplierArray) { if (!timeTrajectory.empty()) { for (size_t i = 0; i < observingTimePoints.size(); i++) { const auto t = timeTrajectory.front() + observingTimePoints[i]; - const auto indexAlpha = LinearInterpolation::timeSegment(t, timeTrajectory); + const auto indexAlpha = + LinearInterpolation::timeSegment(t, timeTrajectory); const auto multiplier = [&]() -> Multiplier { switch (interpolationStrategy) { case CallbackInterpolationStrategy::nearest_time: - return (indexAlpha.second > 0.5) ? static_cast(termMultiplierArray[indexAlpha.first]) - : static_cast(termMultiplierArray[indexAlpha.first + 1]); + return (indexAlpha.second > 0.5) + ? static_cast( + termMultiplierArray[indexAlpha.first]) + : static_cast( + termMultiplierArray[indexAlpha.first + 1]); case CallbackInterpolationStrategy::linear_interpolation: - return LinearInterpolation::interpolate(indexAlpha, termMultiplierArray); + return LinearInterpolation::interpolate(indexAlpha, + termMultiplierArray); default: - throw std::runtime_error("[createMultiplierCallback] This CallbackInterpolationStrategy is not implemented!"); + throw std::runtime_error( + "[createMultiplierCallback] This " + "CallbackInterpolationStrategy is not implemented!"); } }(); - multiplierPublishers[i].publish(ros_msg_conversions::createMultiplierMsg(t, multiplier)); + multiplierPublishers[i]->publish( + ros_msg_conversions::createMultiplierMsg(t, multiplier)); } } }; diff --git a/ocs2_ros_interfaces/src/visualization/VisualizationHelpers.cpp b/ocs2_ros_interfaces/src/visualization/VisualizationHelpers.cpp index d95726ac5..15d450958 100644 --- a/ocs2_ros_interfaces/src/visualization/VisualizationHelpers.cpp +++ b/ocs2_ros_interfaces/src/visualization/VisualizationHelpers.cpp @@ -31,9 +31,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { -std_msgs::ColorRGBA getColor(Color color, double alpha) { +std_msgs::msg::ColorRGBA getColor(Color color, double alpha) { const auto rgb = getRGB(color); - std_msgs::ColorRGBA colorMsg; + std_msgs::msg::ColorRGBA colorMsg; colorMsg.r = rgb[0]; colorMsg.g = rgb[1]; colorMsg.b = rgb[2]; @@ -41,24 +41,24 @@ std_msgs::ColorRGBA getColor(Color color, double alpha) { return colorMsg; } -void setVisible(visualization_msgs::Marker& marker) { +void setVisible(visualization_msgs::msg::Marker& marker) { marker.color.a = 1.0; } -void setInvisible(visualization_msgs::Marker& marker) { +void setInvisible(visualization_msgs::msg::Marker& marker) { marker.color.a = 0.001; // Rviz creates a warning when a is set to 0 } -std_msgs::Header getHeaderMsg(const std::string& frame_id, const ros::Time& timeStamp) { - std_msgs::Header header; +std_msgs::msg::Header getHeaderMsg(const std::string& frame_id, const rclcpp::Time& timeStamp) { + std_msgs::msg::Header header; header.frame_id = frame_id; header.stamp = timeStamp; return header; } -visualization_msgs::Marker getLineMsg(std::vector&& points, Color color, double lineWidth) { - visualization_msgs::Marker line; - line.type = visualization_msgs::Marker::LINE_STRIP; +visualization_msgs::msg::Marker getLineMsg(std::vector&& points, Color color, double lineWidth) { + visualization_msgs::msg::Marker line; + line.type = visualization_msgs::msg::Marker::LINE_STRIP; line.scale.x = lineWidth; line.color = getColor(color); line.points = std::move(points); @@ -66,24 +66,24 @@ visualization_msgs::Marker getLineMsg(std::vector&& points return line; } -geometry_msgs::Point getPointMsg(const Eigen::Vector3d& point) { - geometry_msgs::Point pointMsg; +geometry_msgs::msg::Point getPointMsg(const Eigen::Vector3d& point) { + geometry_msgs::msg::Point pointMsg; pointMsg.x = point.x(); pointMsg.y = point.y(); pointMsg.z = point.z(); return pointMsg; } -geometry_msgs::Vector3 getVectorMsg(const Eigen::Vector3d& vec) { - geometry_msgs::Vector3 vecMsg; +geometry_msgs::msg::Vector3 getVectorMsg(const Eigen::Vector3d& vec) { + geometry_msgs::msg::Vector3 vecMsg; vecMsg.x = vec.x(); vecMsg.y = vec.y(); vecMsg.z = vec.z(); return vecMsg; } -geometry_msgs::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientation) { - geometry_msgs::Quaternion orientationMsg; +geometry_msgs::msg::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientation) { + geometry_msgs::msg::Quaternion orientationMsg; orientationMsg.x = orientation.x(); orientationMsg.y = orientation.y(); orientationMsg.z = orientation.z(); @@ -91,9 +91,9 @@ geometry_msgs::Quaternion getOrientationMsg(const Eigen::Quaterniond& orientatio return orientationMsg; } -visualization_msgs::Marker getSphereMsg(const Eigen::Vector3d& point, Color color, double diameter) { - visualization_msgs::Marker sphere; - sphere.type = visualization_msgs::Marker::SPHERE; +visualization_msgs::msg::Marker getSphereMsg(const Eigen::Vector3d& point, Color color, double diameter) { + visualization_msgs::msg::Marker sphere; + sphere.type = visualization_msgs::msg::Marker::SPHERE; sphere.pose.position = getPointMsg(point); sphere.pose.orientation = getOrientationMsg({1., 0., 0., 0.}); sphere.scale.x = diameter; @@ -103,10 +103,10 @@ visualization_msgs::Marker getSphereMsg(const Eigen::Vector3d& point, Color colo return sphere; } -visualization_msgs::Marker getPlaneMsg(const Eigen::Vector3d& point, const Eigen::Quaterniond& orientation, Color color, double width, +visualization_msgs::msg::Marker getPlaneMsg(const Eigen::Vector3d& point, const Eigen::Quaterniond& orientation, Color color, double width, double length, double thickness) { - visualization_msgs::Marker plane; - plane.type = visualization_msgs::Marker::CUBE; + visualization_msgs::msg::Marker plane; + plane.type = visualization_msgs::msg::Marker::CUBE; plane.pose.position = getPointMsg(point); plane.pose.orientation = getOrientationMsg(orientation); plane.scale.x = length; @@ -116,17 +116,17 @@ visualization_msgs::Marker getPlaneMsg(const Eigen::Vector3d& point, const Eigen return plane; } -visualization_msgs::Marker getArrowToPointMsg(const Eigen::Vector3d& vec, const Eigen::Vector3d& point, Color color) { +visualization_msgs::msg::Marker getArrowToPointMsg(const Eigen::Vector3d& vec, const Eigen::Vector3d& point, Color color) { return getArrowBetweenPointsMsg(point - vec, point, color); } -visualization_msgs::Marker getArrowAtPointMsg(const Eigen::Vector3d& vec, const Eigen::Vector3d& point, Color color) { +visualization_msgs::msg::Marker getArrowAtPointMsg(const Eigen::Vector3d& vec, const Eigen::Vector3d& point, Color color) { return getArrowBetweenPointsMsg(point, point + vec, color); } -visualization_msgs::Marker getArrowBetweenPointsMsg(const Eigen::Vector3d& start, const Eigen::Vector3d& end, Color color) { - visualization_msgs::Marker arrow; - arrow.type = visualization_msgs::Marker::ARROW; +visualization_msgs::msg::Marker getArrowBetweenPointsMsg(const Eigen::Vector3d& start, const Eigen::Vector3d& end, Color color) { + visualization_msgs::msg::Marker arrow; + arrow.type = visualization_msgs::msg::Marker::ARROW; arrow.scale.x = 0.01; // shaft diameter arrow.scale.y = 0.02; // arrow-head diameter arrow.scale.z = 0.06; // arrow-head length @@ -138,7 +138,7 @@ visualization_msgs::Marker getArrowBetweenPointsMsg(const Eigen::Vector3d& start return arrow; } -visualization_msgs::Marker getFootMarker(const Eigen::Vector3d& position, bool contactFlag, Color color, double diameter, +visualization_msgs::msg::Marker getFootMarker(const Eigen::Vector3d& position, bool contactFlag, Color color, double diameter, double liftedAlpha) { auto footMarker = getSphereMsg(position, color, diameter); if (!contactFlag) { @@ -148,7 +148,7 @@ visualization_msgs::Marker getFootMarker(const Eigen::Vector3d& position, bool c return footMarker; } -visualization_msgs::Marker getForceMarker(const Eigen::Vector3d& force, const Eigen::Vector3d& position, bool contactFlag, Color color, +visualization_msgs::msg::Marker getForceMarker(const Eigen::Vector3d& force, const Eigen::Vector3d& position, bool contactFlag, Color color, double forceScale) { auto forceMarker = getArrowToPointMsg(force / forceScale, position, color); forceMarker.ns = "EE Forces"; diff --git a/ocs2_ros_interfaces/test/test_custom_callback_queue.cpp b/ocs2_ros_interfaces/test/test_custom_callback_queue.cpp index e7446261c..79376c625 100644 --- a/ocs2_ros_interfaces/test/test_custom_callback_queue.cpp +++ b/ocs2_ros_interfaces/test/test_custom_callback_queue.cpp @@ -29,62 +29,49 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include -#include -#include "std_msgs/String.h" +#include +#include // Global queue: will be available as a member variable -ros::CallbackQueue my_queue; +rclcpp::CallbackGroup::SharedPtr callback_group_subscriber; -void chatterCallback(const std_msgs::String::ConstPtr& msg) { - printf("I heard: [%s]\n", msg->data.c_str()); -} - -void subscriberWorker() { - ros::Rate loop_rate(30); - while (ros::ok()) { - std::cout << "Check for new messages" << std::endl; - my_queue.callOne(); - loop_rate.sleep(); - } +void chatterCallback(const std_msgs::msg::String& msg) { + printf("I heard: [%s]\n", msg.data.c_str()); } int main(int argc, char* argv[]) { - ros::init(argc, argv, "my_node"); + rclcpp::init(argc, argv); + rclcpp::executors::MultiThreadedExecutor executor; // Publisher - ros::NodeHandle nh_pub; + std::shared_ptr node_pub = rclcpp::Node::make_shared("test_custom_callback_publisher"); size_t publish_queue_size = 1000; - ros::Publisher chatter_pub = nh_pub.advertise("chatter", publish_queue_size); + rclcpp::Publisher::SharedPtr chatter_pub = node_pub->create_publisher("chatter", publish_queue_size); // Subscriber - ros::NodeHandle nh_sub; - nh_sub.setCallbackQueue(&my_queue); - + std::shared_ptr node_sub = rclcpp::Node::make_shared("test_custom_callback_subscriber"); size_t subscribe_queue_size = 1000; - ros::Subscriber sub = nh_sub.subscribe("chatter", subscribe_queue_size, chatterCallback); - - auto subscriberThread = std::thread(&subscriberWorker); + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = node_sub->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::Subscription::SharedPtr sub = node_sub->create_subscription("chatter", subscribe_queue_size, &chatterCallback, sub_opt); - ros::Rate loop_rate(10); + executor.add_node(node_pub); + executor.add_node(node_sub); + rclcpp::Rate loop_rate(10); int count = 0; - while (ros::ok()) { - std_msgs::String msg; + while (rclcpp::ok()) { + std_msgs::msg::String msg; std::stringstream ss; ss << "hello world " << count; msg.data = ss.str(); - chatter_pub.publish(msg); - ros::spinOnce(); + chatter_pub->publish(msg); + executor.spin_once(); loop_rate.sleep(); ++count; } - if (subscriberThread.joinable()) { - subscriberThread.join(); - }; - return 0; } From ce672ac1664c7933ef46103f20aea622a0406ca3 Mon Sep 17 00:00:00 2001 From: "Yu, Yan" Date: Tue, 2 Sep 2025 10:27:50 +0800 Subject: [PATCH 33/42] Fix compiling warnings found in CI (#47) --- ocs2_ddp/CMakeLists.txt | 2 +- ocs2_ros_interfaces/package.xml | 2 -- ocs2_test_tools/ocs2_qp_solver/CMakeLists.txt | 2 +- ocs2_test_tools/ocs2_qp_solver/package.xml | 1 - 4 files changed, 2 insertions(+), 5 deletions(-) diff --git a/ocs2_ddp/CMakeLists.txt b/ocs2_ddp/CMakeLists.txt index 06904af73..98603528a 100644 --- a/ocs2_ddp/CMakeLists.txt +++ b/ocs2_ddp/CMakeLists.txt @@ -7,7 +7,7 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter -Wno-sign-compare -Wno-return-type -Wno-unused-variable -Wno-unused-but-set-variable) endif() ## Find ament macros and libraries diff --git a/ocs2_ros_interfaces/package.xml b/ocs2_ros_interfaces/package.xml index af9563d9a..058bb790c 100644 --- a/ocs2_ros_interfaces/package.xml +++ b/ocs2_ros_interfaces/package.xml @@ -16,7 +16,6 @@ eigen3_cmake_module cmake_modules - cmake_clang_tools eigen rclcpp @@ -27,7 +26,6 @@ visualization_msgs geometry_msgs interactive_markers - rqt_multiplot ros2launch diff --git a/ocs2_test_tools/ocs2_qp_solver/CMakeLists.txt b/ocs2_test_tools/ocs2_qp_solver/CMakeLists.txt index 26e180167..9d6d61a00 100644 --- a/ocs2_test_tools/ocs2_qp_solver/CMakeLists.txt +++ b/ocs2_test_tools/ocs2_qp_solver/CMakeLists.txt @@ -6,7 +6,7 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wno-sign-compare) endif() set(CMAKE_EXPORT_COMPILE_COMMANDS ON) diff --git a/ocs2_test_tools/ocs2_qp_solver/package.xml b/ocs2_test_tools/ocs2_qp_solver/package.xml index 325797e67..95f4f0b1f 100644 --- a/ocs2_test_tools/ocs2_qp_solver/package.xml +++ b/ocs2_test_tools/ocs2_qp_solver/package.xml @@ -13,7 +13,6 @@ ament_cmake eigen3_cmake_module eigen3_cmake_module - cmake_clang_tools eigen ocs2_core From 239c3dd8074c09e70b785f90dd1fb58cd3f3107a Mon Sep 17 00:00:00 2001 From: sunausti Date: Tue, 2 Sep 2025 08:27:00 +0000 Subject: [PATCH 34/42] ocs2_self_collision: porting to ros2 humble (#45) * ocs2_self_collision: porting to ros2 humble --------- Signed-off-by: Austin Sun --- .../ocs2_self_collision/CMakeLists.txt | 107 +++++++++--------- .../ocs2_self_collision/COLCON_IGNORE | 0 .../ocs2_self_collision/package.xml | 18 ++- .../src/PinocchioGeometryInterface.cpp | 1 - 4 files changed, 68 insertions(+), 58 deletions(-) delete mode 100644 ocs2_pinocchio/ocs2_self_collision/COLCON_IGNORE diff --git a/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt b/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt index 37e06a0e8..124277eee 100644 --- a/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt @@ -1,40 +1,35 @@ -cmake_minimum_required(VERSION 3.0.2) -project(ocs2_self_collision) +cmake_minimum_required(VERSION 3.14.4) +project(ocs2_self_collision LANGUAGES CXX) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter -Wno-unused-variable -Wno-return-type -Wno-sign-compare -Wno-deprecated-declarations -Wno-reorder) +endif() # Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -set(CATKIN_PACKAGE_DEPENDENCIES - ocs2_core - ocs2_robotic_tools - ocs2_pinocchio_interface -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) +## Find ament macros and libraries +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) find_package(PkgConfig REQUIRED) pkg_check_modules(pinocchio REQUIRED pinocchio) pkg_check_modules(hpp-fcl REQUIRED hpp-fcl) # requires liboctomap-dev and libassimp-dev -find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - pinocchio -) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(hpp-fcl REQUIRED) +find_package(pinocchio REQUIRED) +find_package(urdf REQUIRED) +find_package(urdfdom REQUIRED) ########### ## Build ## @@ -43,41 +38,41 @@ catkin_package( set(FLAGS ${OCS2_CXX_FLAGS} ${pinocchio_CFLAGS_OTHER} + -DPINOCCHIO_WITH_HPP_FCL + -DPINOCCHIO_ENABLE_COMPATIBILITY_WITH_VERSION_2 -Wno-ignored-attributes - -Wno-invalid-partial-specialization # to silence warning with unsupported Eigen Tensor -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR ) -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${pinocchio_INCLUDE_DIRS} - ${hpp-fcl_INCLUDE_DIRS} -) - link_directories( ${pinocchio_LIBRARY_DIRS} ) -# ocs2 pinocchio interface library -add_library(${PROJECT_NAME} +# ocs2 self collision library +add_library(${PROJECT_NAME} SHARED src/PinocchioGeometryInterface.cpp src/SelfCollision.cpp src/SelfCollisionCppAd.cpp src/SelfCollisionConstraint.cpp src/SelfCollisionConstraintCppAd.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} + +set(includes_${PROJECT_NAME} + $ + $ ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${pinocchio_LIBRARIES} + + +target_include_directories(${PROJECT_NAME} PUBLIC + ${includes_${PROJECT_NAME}} ) + target_compile_options(${PROJECT_NAME} PUBLIC ${FLAGS}) +# Link ament dependencies +ament_target_dependencies(${PROJECT_NAME} ocs2_core ocs2_robotic_tools pinocchio hpp-fcl ocs2_pinocchio_interface) + #################### ## Clang tooling ### #################### @@ -97,15 +92,23 @@ endif (cmake_clang_tools_FOUND) ## Install ## ############# -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# Install library +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + +# Install headers +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} ) -############ -# Testing ## -############ +# Export targets +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(ocs2_core ocs2_robotic_tools ocs2_pinocchio_interface pinocchio Eigen3 hpp-fcl::hpp-fcl) + +ament_package() diff --git a/ocs2_pinocchio/ocs2_self_collision/COLCON_IGNORE b/ocs2_pinocchio/ocs2_self_collision/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/ocs2_pinocchio/ocs2_self_collision/package.xml b/ocs2_pinocchio/ocs2_self_collision/package.xml index 4935695bb..87260d414 100644 --- a/ocs2_pinocchio/ocs2_self_collision/package.xml +++ b/ocs2_pinocchio/ocs2_self_collision/package.xml @@ -1,5 +1,5 @@ - + ocs2_self_collision 0.0.0 The ocs2_self_collision package @@ -7,14 +7,22 @@ Michael Spieler Perry Franklin - TODO - - catkin - cmake_clang_tools + BSD + ament_cmake ocs2_core ocs2_robotic_tools ocs2_pinocchio_interface pinocchio + hpp-fcl + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/ocs2_pinocchio/ocs2_self_collision/src/PinocchioGeometryInterface.cpp b/ocs2_pinocchio/ocs2_self_collision/src/PinocchioGeometryInterface.cpp index 41bf223a9..8af250ba9 100644 --- a/ocs2_pinocchio/ocs2_self_collision/src/PinocchioGeometryInterface.cpp +++ b/ocs2_pinocchio/ocs2_self_collision/src/PinocchioGeometryInterface.cpp @@ -34,7 +34,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include #include #include From 54d648b353fbcf538ba1ac18927e128b458960df Mon Sep 17 00:00:00 2001 From: "Yu, Yan" Date: Fri, 5 Sep 2025 10:36:15 +0800 Subject: [PATCH 35/42] Fix hpp-fcl export from ocs2_self_collision (#50) --- ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt b/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt index 124277eee..9ce0f7aac 100644 --- a/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_self_collision/CMakeLists.txt @@ -109,6 +109,6 @@ install( # Export targets ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies(ocs2_core ocs2_robotic_tools ocs2_pinocchio_interface pinocchio Eigen3 hpp-fcl::hpp-fcl) +ament_export_dependencies(ocs2_core ocs2_robotic_tools ocs2_pinocchio_interface pinocchio Eigen3 hpp-fcl) ament_package() From 1b49304a0d6bf629d3578078bfa568b5349fe031 Mon Sep 17 00:00:00 2001 From: baihe-liu Date: Mon, 8 Sep 2025 10:51:20 +0800 Subject: [PATCH 36/42] ocs2_mobile_manipulator: porting to ros2 humble (#49) * porting ocs2_mobile_manipulator --- .../{ => ocs2_ballbot}/COLCON_IGNORE | 0 .../ocs2_ballbot_ros/COLCON_IGNORE | 0 .../ocs2_cartpole/COLCON_IGNORE | 0 .../ocs2_cartpole_ros/COLCON_IGNORE | 0 .../ocs2_double_integrator/COLCON_IGNORE | 0 .../ocs2_double_integrator_ros/COLCON_IGNORE | 0 .../ocs2_legged_robot/COLCON_IGNORE | 0 .../ocs2_legged_robot_ros/COLCON_IGNORE | 0 .../ocs2_mobile_manipulator/CMakeLists.txt | 188 ++++++++++-------- .../ocs2_mobile_manipulator/package.xml | 19 +- .../ocs2_mobile_manipulator_ros/COLCON_IGNORE | 0 .../ocs2_perceptive_anymal/COLCON_IGNORE | 0 .../ocs2_quadrotor/COLCON_IGNORE | 0 .../ocs2_quadrotor_ros/COLCON_IGNORE | 0 .../ocs2_robotic_examples/COLCON_IGNORE | 0 15 files changed, 123 insertions(+), 84 deletions(-) rename ocs2_robotic_examples/{ => ocs2_ballbot}/COLCON_IGNORE (100%) create mode 100644 ocs2_robotic_examples/ocs2_ballbot_ros/COLCON_IGNORE create mode 100644 ocs2_robotic_examples/ocs2_cartpole/COLCON_IGNORE create mode 100644 ocs2_robotic_examples/ocs2_cartpole_ros/COLCON_IGNORE create mode 100644 ocs2_robotic_examples/ocs2_double_integrator/COLCON_IGNORE create mode 100644 ocs2_robotic_examples/ocs2_double_integrator_ros/COLCON_IGNORE create mode 100644 ocs2_robotic_examples/ocs2_legged_robot/COLCON_IGNORE create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_ros/COLCON_IGNORE create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/COLCON_IGNORE create mode 100644 ocs2_robotic_examples/ocs2_perceptive_anymal/COLCON_IGNORE create mode 100644 ocs2_robotic_examples/ocs2_quadrotor/COLCON_IGNORE create mode 100644 ocs2_robotic_examples/ocs2_quadrotor_ros/COLCON_IGNORE create mode 100644 ocs2_robotic_examples/ocs2_robotic_examples/COLCON_IGNORE diff --git a/ocs2_robotic_examples/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_ballbot/COLCON_IGNORE similarity index 100% rename from ocs2_robotic_examples/COLCON_IGNORE rename to ocs2_robotic_examples/ocs2_ballbot/COLCON_IGNORE diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_ballbot_ros/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_examples/ocs2_cartpole/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_cartpole/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_cartpole_ros/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_examples/ocs2_double_integrator/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_double_integrator/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_double_integrator_ros/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_examples/ocs2_legged_robot/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_legged_robot/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_legged_robot_ros/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/CMakeLists.txt b/ocs2_robotic_examples/ocs2_mobile_manipulator/CMakeLists.txt index 9d010712d..200f77f32 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/CMakeLists.txt @@ -1,10 +1,33 @@ -cmake_minimum_required(VERSION 3.0.2) -project(ocs2_mobile_manipulator) +cmake_minimum_required(VERSION 3.14.4) +project(ocs2_mobile_manipulator LANGUAGES CXX) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wno-extra -Wpedantic -Wno-reorder -Wno-unused-function) +endif() -# Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -set(CATKIN_PACKAGE_DEPENDENCIES +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) +find_package(ocs2_self_collision REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(Boost REQUIRED COMPONENTS system filesystem) +find_package(PkgConfig REQUIRED) +pkg_check_modules(pinocchio REQUIRED pinocchio) +pkg_check_modules(hpp-fcl REQUIRED hpp-fcl) +find_package(urdf REQUIRED) +find_package(urdfdom REQUIRED) + +set(dependencies ocs2_core ocs2_ddp ocs2_mpc @@ -12,37 +35,11 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_robotic_assets ocs2_pinocchio_interface ocs2_self_collision -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(pinocchio REQUIRED pinocchio) - -find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost - pinocchio + Eigen3 + Boost + urdf + urdfdom + hpp-fcl ) ########### @@ -58,26 +55,21 @@ set(FLAGS -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR ) +link_directories( + ${pinocchio_LIBRARY_DIRS} +) + # Resolve for the package path at compile time. configure_file ( "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h.in" "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" @ONLY ) -# Add directories for all targets -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -link_directories( - ${pinocchio_LIBRARY_DIRS} +set(includes_${PROJECT_NAME} + $ + $ ) - -# mobile maniulator interface library -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED src/FactoryFunctions.cpp src/MobileManipulatorPreComputation.cpp src/MobileManipulatorPinocchioMapping.cpp @@ -88,13 +80,11 @@ add_library(${PROJECT_NAME} src/dynamics/FullyActuatedFloatingArmManipulatorDynamics.cpp src/MobileManipulatorInterface.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} +target_include_directories(${PROJECT_NAME} PUBLIC + ${includes_${PROJECT_NAME}} ) -target_link_libraries(${PROJECT_NAME} - ${Boost_LIBRARIES} - ${catkin_LIBRARIES} - ${pinocchio_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} ) target_compile_options(${PROJECT_NAME} PUBLIC ${FLAGS}) @@ -103,8 +93,8 @@ target_compile_options(${PROJECT_NAME} PUBLIC ${FLAGS}) #################### find_package(cmake_clang_tools QUIET) -if (cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling") +if(cmake_clang_tools_FOUND) + message(STATUS "Run clang tooling for target " ${PROJECT_NAME}) add_clang_tooling( TARGETS ${PROJECT_NAME} SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include @@ -117,38 +107,74 @@ endif (cmake_clang_tools_FOUND) ## Install ## ############# -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} ) -install(DIRECTORY config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +install( + DIRECTORY config + DESTINATION share/${PROJECT_NAME} ) + +ament_export_dependencies(${dependencies}) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) + ############# ## Testing ## ############# +find_package(gtest_vendor REQUIRED) +find_package(ament_cmake_gtest REQUIRED) -# Helper macro for adding target applications -macro(add_ocs2_test APP_NAME APP_SRC) - catkin_add_gtest(${APP_NAME} - ${APP_SRC} - ) - target_include_directories(${APP_NAME} PRIVATE - ${PROJECT_BINARY_DIR}/include - ) - target_link_libraries(${APP_NAME} - gtest_main - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} +set(test_includes_${PROJECT_NAME} + ${includes_${PROJECT_NAME}} + $ + $ +) + +function(ament_add_gtest_wrapper test_name) + ament_add_gtest(${test_name} ${ARGN}) + target_include_directories(${test_name} PUBLIC + ${test_includes_${PROJECT_NAME}} ) -endmacro() + set_tests_properties(${test_name} PROPERTIES TIMEOUT 150) +endfunction() + +ament_add_gtest_wrapper(test_self_collision + test/testSelfCollision.cpp +) +ament_target_dependencies(test_self_collision + ${dependencies} +) +target_link_libraries(test_self_collision + ${PROJECT_NAME} +) + +ament_add_gtest_wrapper(test_end_effector_constraint + test/testEndEffectorConstraint.cpp +) +ament_target_dependencies(test_end_effector_constraint + ${dependencies} +) +target_link_libraries(test_end_effector_constraint + ${PROJECT_NAME} +) + +ament_add_gtest_wrapper(test_dummy_mobile_manipulator + test/testDummyMobileManipulator.cpp +) +ament_target_dependencies(test_dummy_mobile_manipulator + ${dependencies} +) +target_link_libraries(test_dummy_mobile_manipulator + ${PROJECT_NAME} +) -add_ocs2_test(SelfCollisionTest test/testSelfCollision.cpp) -add_ocs2_test(EndEffectorConstraintTest test/testEndEffectorConstraint.cpp) -add_ocs2_test(DummyMobileManipulatorTest test/testDummyMobileManipulator.cpp) +ament_package() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/package.xml b/ocs2_robotic_examples/ocs2_mobile_manipulator/package.xml index 0d1ff99af..930de6a84 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/package.xml +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/package.xml @@ -1,5 +1,5 @@ - + ocs2_mobile_manipulator 0.0.0 The ocs2_mobile_manipulator package @@ -8,9 +8,12 @@ Perry Franklin Mayank Mittal - TODO + BSD + + ament_cmake + eigen3_cmake_module + eigen3_cmake_module - catkin cmake_clang_tools ocs2_core @@ -21,5 +24,15 @@ ocs2_pinocchio_interface ocs2_self_collision pinocchio + eigen + Boost + urdf + urdfdom + hpp-fcl + + ament_cmake_gtest + + ament_cmake + \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_perceptive_anymal/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_examples/ocs2_quadrotor/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_quadrotor/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_quadrotor_ros/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_robotic_examples/ocs2_robotic_examples/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_robotic_examples/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb From 08c3c1b439decf80a24a85d7e9b55ebd7ea8b8d6 Mon Sep 17 00:00:00 2001 From: Nisyhaal Date: Tue, 9 Sep 2025 10:09:37 +0800 Subject: [PATCH 37/42] ocs2_self_collision_visualization: porting to ros2 humble (#46) * feat: Modify CMakeList.txt * feat: Modify package.xml * feat: Modify source and header files * fix: Remove COLCON_IGNORE * Update ocs2_pinocchio/ocs2_self_collision_visualization/include/ocs2_self_collision_visualization/GeometryInterfaceVisualization.h * Update ocs2_pinocchio/ocs2_self_collision_visualization/CMakeLists.txt * fix: Add URDF --- .../CMakeLists.txt | 111 +++++++++--------- .../COLCON_IGNORE | 0 .../GeometryInterfaceVisualization.h | 13 +- .../package.xml | 20 +++- .../src/GeometryInterfaceVisualization.cpp | 30 ++--- 5 files changed, 93 insertions(+), 81 deletions(-) delete mode 100644 ocs2_pinocchio/ocs2_self_collision_visualization/COLCON_IGNORE diff --git a/ocs2_pinocchio/ocs2_self_collision_visualization/CMakeLists.txt b/ocs2_pinocchio/ocs2_self_collision_visualization/CMakeLists.txt index b61f92809..754df5957 100644 --- a/ocs2_pinocchio/ocs2_self_collision_visualization/CMakeLists.txt +++ b/ocs2_pinocchio/ocs2_self_collision_visualization/CMakeLists.txt @@ -1,90 +1,86 @@ -cmake_minimum_required(VERSION 3.0.2) -project(ocs2_self_collision_visualization) +cmake_minimum_required(VERSION 3.14.4) +project(ocs2_self_collision_visualization LANGUAGES CXX) -# Generate compile_commands.json for clang tools +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter -Wno-unused-variable -Wno-return-type -Wno-sign-compare -Wno-deprecated-declarations -Wno-reorder) +endif() set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -set(CATKIN_PACKAGE_DEPENDENCIES +set(dependencies ocs2_core ocs2_robotic_tools ocs2_pinocchio_interface ocs2_self_collision ocs2_ros_interfaces + urdf # visualization deps - roscpp + rclcpp std_msgs geometry_msgs visualization_msgs + pinocchio + hpp-fcl ) -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) +find_package(ament_cmake REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) +find_package(ocs2_self_collision REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) +find_package(urdf REQUIRED) + +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) find_package(PkgConfig REQUIRED) pkg_check_modules(pinocchio REQUIRED pinocchio) pkg_check_modules(hpp-fcl REQUIRED hpp-fcl) -# requires liboctomap-dev and libassimp-dev - -find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - pinocchio -) -########### -## Build ## -########### +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(urdfdom REQUIRED) set(FLAGS ${OCS2_CXX_FLAGS} ${pinocchio_CFLAGS_OTHER} + -DPINOCCHIO_WITH_HPP_FCL -Wno-ignored-attributes -Wno-invalid-partial-specialization # to silence warning with unsupported Eigen Tensor -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR ) -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${pinocchio_INCLUDE_DIRS} - ${hpp-fcl_INCLUDE_DIRS} -) - link_directories( ${pinocchio_LIBRARY_DIRS} ) # ocs2 pinocchio interface library -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED src/GeometryInterfaceVisualization.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} + +set(includes_${PROJECT_NAME} + $ + $ ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${pinocchio_LIBRARIES} + +target_include_directories(${PROJECT_NAME} PUBLIC + ${includes_${PROJECT_NAME}} ) + target_compile_options(${PROJECT_NAME} PUBLIC ${FLAGS}) -#################### -## Clang tooling ### -#################### +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} +) find_package(cmake_clang_tools QUIET) if (cmake_clang_tools_FOUND) @@ -97,15 +93,18 @@ if (cmake_clang_tools_FOUND) ) endif (cmake_clang_tools_FOUND) -############# -## Install ## -############# - -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} ) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${dependencies}) + +ament_package() diff --git a/ocs2_pinocchio/ocs2_self_collision_visualization/COLCON_IGNORE b/ocs2_pinocchio/ocs2_self_collision_visualization/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/ocs2_pinocchio/ocs2_self_collision_visualization/include/ocs2_self_collision_visualization/GeometryInterfaceVisualization.h b/ocs2_pinocchio/ocs2_self_collision_visualization/include/ocs2_self_collision_visualization/GeometryInterfaceVisualization.h index 5fa6ce7d3..4c53647c9 100644 --- a/ocs2_pinocchio/ocs2_self_collision_visualization/include/ocs2_self_collision_visualization/GeometryInterfaceVisualization.h +++ b/ocs2_pinocchio/ocs2_self_collision_visualization/include/ocs2_self_collision_visualization/GeometryInterfaceVisualization.h @@ -29,7 +29,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include +#include + +#include #include #include @@ -38,19 +40,20 @@ namespace ocs2 { class GeometryInterfaceVisualization { public: - GeometryInterfaceVisualization(PinocchioInterface pinocchioInterface, PinocchioGeometryInterface geometryInterface, ros::NodeHandle& nh, + GeometryInterfaceVisualization(rclcpp::Node::SharedPtr node, PinocchioInterface pinocchioInterface, PinocchioGeometryInterface geometryInterface, std::string pinocchioWorldFrame = "world"); virtual ~GeometryInterfaceVisualization() = default; void publishDistances(const ocs2::vector_t&); - private: +private: + rclcpp::Node::SharedPtr node_; PinocchioInterface pinocchioInterface_; PinocchioGeometryInterface geometryInterface_; - ros::Publisher markerPublisher_; + rclcpp::Publisher::SharedPtr markerPublisher_; std::string pinocchioWorldFrame_; }; -} // namespace ocs2 +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_pinocchio/ocs2_self_collision_visualization/package.xml b/ocs2_pinocchio/ocs2_self_collision_visualization/package.xml index 46cb2614b..a4bc651eb 100644 --- a/ocs2_pinocchio/ocs2_self_collision_visualization/package.xml +++ b/ocs2_pinocchio/ocs2_self_collision_visualization/package.xml @@ -1,5 +1,5 @@ - + ocs2_self_collision_visualization 0.0.0 The ocs2_self_collision_visualization package @@ -8,16 +8,26 @@ TODO - catkin - cmake_clang_tools + ament_cmake - roscpp + rclcpp std_msgs geometry_msgs visualization_msgs ocs2_core ocs2_robotic_tools ocs2_pinocchio_interface + pinocchio + hpp-fcl ocs2_ros_interfaces ocs2_self_collision - + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + + \ No newline at end of file diff --git a/ocs2_pinocchio/ocs2_self_collision_visualization/src/GeometryInterfaceVisualization.cpp b/ocs2_pinocchio/ocs2_self_collision_visualization/src/GeometryInterfaceVisualization.cpp index 291bacd40..c457d7a19 100644 --- a/ocs2_pinocchio/ocs2_self_collision_visualization/src/GeometryInterfaceVisualization.cpp +++ b/ocs2_pinocchio/ocs2_self_collision_visualization/src/GeometryInterfaceVisualization.cpp @@ -32,8 +32,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include - #include #include "ocs2_self_collision_visualization/GeometryInterfaceVisualization.h" @@ -43,12 +41,14 @@ namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -GeometryInterfaceVisualization::GeometryInterfaceVisualization(PinocchioInterface pinocchioInterface, - PinocchioGeometryInterface geometryInterface, ros::NodeHandle& nh, +GeometryInterfaceVisualization::GeometryInterfaceVisualization(rclcpp::Node::SharedPtr node, + PinocchioInterface pinocchioInterface, + PinocchioGeometryInterface geometryInterface, std::string pinocchioWorldFrame) - : pinocchioInterface_(std::move(pinocchioInterface)), + : node_(node), + pinocchioInterface_(std::move(pinocchioInterface)), geometryInterface_(std::move(geometryInterface)), - markerPublisher_(nh.advertise("distance_markers", 1, true)), + markerPublisher_(node_->create_publisher("distance_markers", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local())), pinocchioWorldFrame_(std::move(pinocchioWorldFrame)) {} /******************************************************************************************************/ @@ -60,14 +60,14 @@ void GeometryInterfaceVisualization::publishDistances(const ocs2::vector_t& q) { pinocchio::forwardKinematics(model, data, q); const auto results = geometryInterface_.computeDistances(pinocchioInterface_); - visualization_msgs::MarkerArray markerArray; + visualization_msgs::msg::MarkerArray markerArray; constexpr size_t numMarkersPerResult = 5; - visualization_msgs::Marker markerTemplate; + visualization_msgs::msg::Marker markerTemplate; markerTemplate.color = ros_msg_helpers::getColor({0, 1, 0}, 1); markerTemplate.header.frame_id = pinocchioWorldFrame_; - markerTemplate.header.stamp = ros::Time::now(); + markerTemplate.header.stamp = node_->now(); markerTemplate.pose.orientation = ros_msg_helpers::getOrientationMsg({1, 0, 0, 0}); markerArray.markers.resize(results.size() * numMarkersPerResult, markerTemplate); @@ -81,7 +81,7 @@ void GeometryInterfaceVisualization::publishDistances(const ocs2::vector_t& q) { } // The actual distance line, also denoting direction of the distance - markerArray.markers[numMarkersPerResult * i].type = visualization_msgs::Marker::ARROW; + markerArray.markers[numMarkersPerResult * i].type = visualization_msgs::msg::Marker::ARROW; markerArray.markers[numMarkersPerResult * i].points.push_back(ros_msg_helpers::getPointMsg(results[i].nearest_points[0])); markerArray.markers[numMarkersPerResult * i].points.push_back(ros_msg_helpers::getPointMsg(results[i].nearest_points[1])); markerArray.markers[numMarkersPerResult * i].id = numMarkersPerResult * i; @@ -89,7 +89,7 @@ void GeometryInterfaceVisualization::publishDistances(const ocs2::vector_t& q) { markerArray.markers[numMarkersPerResult * i].scale.y = 0.02; markerArray.markers[numMarkersPerResult * i].scale.z = 0.04; // Dots at the end of the arrow, denoting the close locations on the body - markerArray.markers[numMarkersPerResult * i + 1].type = visualization_msgs::Marker::SPHERE_LIST; + markerArray.markers[numMarkersPerResult * i + 1].type = visualization_msgs::msg::Marker::SPHERE_LIST; markerArray.markers[numMarkersPerResult * i + 1].points.push_back(ros_msg_helpers::getPointMsg(results[i].nearest_points[0])); markerArray.markers[numMarkersPerResult * i + 1].points.push_back(ros_msg_helpers::getPointMsg(results[i].nearest_points[1])); markerArray.markers[numMarkersPerResult * i + 1].scale.x = 0.02; @@ -98,14 +98,14 @@ void GeometryInterfaceVisualization::publishDistances(const ocs2::vector_t& q) { markerArray.markers[numMarkersPerResult * i + 1].id = numMarkersPerResult * i + 1; // Text denoting the object number in the geometry model, raised above the spheres markerArray.markers[numMarkersPerResult * i + 2].id = numMarkersPerResult * i + 2; - markerArray.markers[numMarkersPerResult * i + 2].type = visualization_msgs::Marker::TEXT_VIEW_FACING; + markerArray.markers[numMarkersPerResult * i + 2].type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; markerArray.markers[numMarkersPerResult * i + 2].scale.z = 0.02; markerArray.markers[numMarkersPerResult * i + 2].pose.position = ros_msg_helpers::getPointMsg(results[i].nearest_points[0]); markerArray.markers[numMarkersPerResult * i + 2].pose.position.z += 0.015; markerArray.markers[numMarkersPerResult * i + 2].text = "obj " + std::to_string(geometryInterface_.getGeometryModel().collisionPairs[i].first); markerArray.markers[numMarkersPerResult * i + 3].id = numMarkersPerResult * i + 3; - markerArray.markers[numMarkersPerResult * i + 3].type = visualization_msgs::Marker::TEXT_VIEW_FACING; + markerArray.markers[numMarkersPerResult * i + 3].type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; markerArray.markers[numMarkersPerResult * i + 3].pose.position = ros_msg_helpers::getPointMsg(results[i].nearest_points[1]); markerArray.markers[numMarkersPerResult * i + 3].pose.position.z += 0.015; markerArray.markers[numMarkersPerResult * i + 3].text = @@ -113,7 +113,7 @@ void GeometryInterfaceVisualization::publishDistances(const ocs2::vector_t& q) { markerArray.markers[numMarkersPerResult * i + 3].scale.z = 0.02; // Text above the arrow, denoting the distance markerArray.markers[numMarkersPerResult * i + 4].id = numMarkersPerResult * i + 4; - markerArray.markers[numMarkersPerResult * i + 4].type = visualization_msgs::Marker::TEXT_VIEW_FACING; + markerArray.markers[numMarkersPerResult * i + 4].type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; markerArray.markers[numMarkersPerResult * i + 4].pose.position = ros_msg_helpers::getPointMsg((results[i].nearest_points[0] + results[i].nearest_points[1]) / 2.0); markerArray.markers[numMarkersPerResult * i + 4].pose.position.z += 0.015; @@ -123,7 +123,7 @@ void GeometryInterfaceVisualization::publishDistances(const ocs2::vector_t& q) { markerArray.markers[numMarkersPerResult * i + 4].scale.z = 0.02; } - markerPublisher_.publish(markerArray); + markerPublisher_->publish(markerArray); } } // namespace ocs2 From 5d8d72334235c96d5d025227b2a875e627773f2f Mon Sep 17 00:00:00 2001 From: sunausti Date: Thu, 11 Sep 2025 00:01:29 +0000 Subject: [PATCH 38/42] ocs2_mobile_manipulator_ros: porting to ros2 humble (#51) * ocs2_mobile_manipulator_ros: porting to ros2 humble Signed-off-by: Austin Sun --- .../CMakeLists.txt | 148 ++++++++++-------- .../ocs2_mobile_manipulator_ros/COLCON_IGNORE | 0 .../MobileManipulatorDummyVisualization.h | 29 ++-- .../launch/manipulator_franka.launch.py | 71 +++++++++ .../launch/manipulator_kinova.launch.py | 46 ++++++ .../launch/manipulator_mabi_mobile.launch.py | 80 ++++++++++ .../launch/manipulator_pr2.launch.py | 46 ++++++ .../manipulator_ridgeback_ur5.launch.py | 80 ++++++++++ .../launch/mobile_manipulator.launch.py | 127 +++++++++++++++ .../launch/visualize.launch.py | 62 ++++++++ .../ocs2_mobile_manipulator_ros/package.xml | 19 ++- ...MobileManipulatorDistanceVisualization.cpp | 33 ++-- .../src/MobileManipulatorDummyMRT.cpp | 28 ++-- .../MobileManipulatorDummyVisualization.cpp | 101 +++++++----- .../src/MobileManipulatorMpcNode.cpp | 27 ++-- .../src/MobileManipulatorTarget.cpp | 13 +- 16 files changed, 755 insertions(+), 155 deletions(-) delete mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/COLCON_IGNORE create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_franka.launch.py create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova.launch.py create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_mabi_mobile.launch.py create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_pr2.launch.py create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_ridgeback_ur5.launch.py create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/mobile_manipulator.launch.py create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/visualize.launch.py diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/CMakeLists.txt index 6831c0606..c7a45ec12 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/CMakeLists.txt @@ -1,12 +1,23 @@ -cmake_minimum_required(VERSION 3.0.2) -project(ocs2_mobile_manipulator_ros) +cmake_minimum_required(VERSION 3.14.4) +project(ocs2_mobile_manipulator_ros LANGUAGES CXX) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter -Wno-unused-variable -Wno-return-type -Wno-sign-compare -Wno-deprecated-declarations -Wno-reorder) +endif() # Generate compile_commands.json for clang tools set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -set(CATKIN_PACKAGE_DEPENDENCIES - roslib - tf +set(dependencies + rclcpp + tf2 + tf2_ros + tf2_geometry_msgs urdf kdl_parser robot_state_publisher @@ -23,35 +34,38 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_self_collision_visualization ocs2_mobile_manipulator ) +## Find ament macros and libraries +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(urdf REQUIRED) +find_package(kdl_parser REQUIRED) +find_package(robot_state_publisher REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(ocs2_ros_interfaces REQUIRED) +find_package(ocs2_core REQUIRED) +find_package(ocs2_ddp REQUIRED) +find_package(ocs2_mpc REQUIRED) +find_package(ocs2_robotic_tools REQUIRED) +find_package(ocs2_robotic_assets REQUIRED) +find_package(ocs2_pinocchio_interface REQUIRED) +find_package(ocs2_self_collision REQUIRED) +find_package(ocs2_self_collision_visualization REQUIRED) +find_package(ocs2_mobile_manipulator REQUIRED) -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -find_package(Boost REQUIRED COMPONENTS - system - filesystem -) find_package(PkgConfig REQUIRED) pkg_check_modules(pinocchio REQUIRED pinocchio) -find_package(Eigen3 3.3 REQUIRED NO_MODULE) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Boost - pinocchio -) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(hpp-fcl REQUIRED) +find_package(pinocchio REQUIRED) +find_package(urdf REQUIRED) +find_package(urdfdom REQUIRED) ########### ## Build ## @@ -66,13 +80,6 @@ set(FLAGS -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR ) -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${pinocchio_INCLUDE_DIRS} -) link_directories( ${pinocchio_LIBRARY_DIRS} @@ -82,51 +89,53 @@ link_directories( add_executable(mobile_manipulator_mpc_node src/MobileManipulatorMpcNode.cpp ) -add_dependencies(mobile_manipulator_mpc_node - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(mobile_manipulator_mpc_node - ${catkin_LIBRARIES} -) + +target_include_directories(mobile_manipulator_mpc_node + PUBLIC + "$" + "$") + target_compile_options(mobile_manipulator_mpc_node PUBLIC ${FLAGS}) +# Use ament for dependency management +ament_target_dependencies(mobile_manipulator_mpc_node ${dependencies}) + # DistanceVisualization node add_executable(mobile_manipulator_distance_visualization src/MobileManipulatorDistanceVisualization.cpp ) -add_dependencies(mobile_manipulator_distance_visualization - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(mobile_manipulator_distance_visualization - ${catkin_LIBRARIES} -) + target_compile_options(mobile_manipulator_distance_visualization PUBLIC ${FLAGS}) +# Use ament for dependency management +ament_target_dependencies(mobile_manipulator_distance_visualization ${dependencies}) + + # Dummy node add_executable(mobile_manipulator_dummy_mrt_node src/MobileManipulatorDummyMRT.cpp src/MobileManipulatorDummyVisualization.cpp ) -add_dependencies(mobile_manipulator_dummy_mrt_node - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(mobile_manipulator_dummy_mrt_node - ${catkin_LIBRARIES} -) + +target_include_directories(mobile_manipulator_dummy_mrt_node + PUBLIC + "$" + "$") + target_compile_options(mobile_manipulator_dummy_mrt_node PUBLIC ${FLAGS}) +# Use ament for dependency management +ament_target_dependencies(mobile_manipulator_dummy_mrt_node ${dependencies}) + # Target node add_executable(mobile_manipulator_target src/MobileManipulatorTarget.cpp ) -add_dependencies(mobile_manipulator_target - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(mobile_manipulator_target - ${catkin_LIBRARIES} -) target_compile_options(mobile_manipulator_target PUBLIC ${FLAGS}) +# Use ament for dependency management +ament_target_dependencies(mobile_manipulator_target ${dependencies}) + #################### ## Clang tooling ### #################### @@ -146,17 +155,26 @@ endif (cmake_clang_tools_FOUND) ## Install ## ############# -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# Install headers +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} ) + +# Install executables install( TARGETS mobile_manipulator_mpc_node mobile_manipulator_distance_visualization mobile_manipulator_dummy_mrt_node mobile_manipulator_target - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + DESTINATION lib/${PROJECT_NAME} ) -install(DIRECTORY launch rviz - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + +# Install launch files and other resources +install( + DIRECTORY launch rviz + DESTINATION share/${PROJECT_NAME} ) + +ament_package() diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/COLCON_IGNORE b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/include/ocs2_mobile_manipulator_ros/MobileManipulatorDummyVisualization.h b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/include/ocs2_mobile_manipulator_ros/MobileManipulatorDummyVisualization.h index aeb71df61..a5f26b28a 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/include/ocs2_mobile_manipulator_ros/MobileManipulatorDummyVisualization.h +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/include/ocs2_mobile_manipulator_ros/MobileManipulatorDummyVisualization.h @@ -29,8 +29,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include -#include +#include +#include +#include +#include #include @@ -43,9 +45,9 @@ namespace mobile_manipulator { class MobileManipulatorDummyVisualization final : public DummyObserver { public: - MobileManipulatorDummyVisualization(ros::NodeHandle& nodeHandle, const MobileManipulatorInterface& interface) - : pinocchioInterface_(interface.getPinocchioInterface()), modelInfo_(interface.getManipulatorModelInfo()) { - launchVisualizerNode(nodeHandle); + MobileManipulatorDummyVisualization(rclcpp::Node::SharedPtr node, const MobileManipulatorInterface& interface) + : node_(node), pinocchioInterface_(interface.getPinocchioInterface()), modelInfo_(interface.getManipulatorModelInfo()) { + launchVisualizerNode(node); } ~MobileManipulatorDummyVisualization() override = default; @@ -53,21 +55,22 @@ class MobileManipulatorDummyVisualization final : public DummyObserver { void update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) override; private: - void launchVisualizerNode(ros::NodeHandle& nodeHandle); + void launchVisualizerNode(rclcpp::Node::SharedPtr node); - void publishObservation(const ros::Time& timeStamp, const SystemObservation& observation); - void publishTargetTrajectories(const ros::Time& timeStamp, const TargetTrajectories& targetTrajectories); - void publishOptimizedTrajectory(const ros::Time& timeStamp, const PrimalSolution& policy); + void publishObservation(const rclcpp::Time& timeStamp, const SystemObservation& observation); + void publishTargetTrajectories(const rclcpp::Time& timeStamp, const TargetTrajectories& targetTrajectories); + void publishOptimizedTrajectory(const rclcpp::Time& timeStamp, const PrimalSolution& policy); + rclcpp::Node::SharedPtr node_; PinocchioInterface pinocchioInterface_; const ManipulatorModelInfo modelInfo_; std::vector removeJointNames_; - + rclcpp::Publisher::SharedPtr jointStatePublisher_; std::unique_ptr robotStatePublisherPtr_; - tf::TransformBroadcaster tfBroadcaster_; + std::unique_ptr tfBroadcaster_; - ros::Publisher stateOptimizedPublisher_; - ros::Publisher stateOptimizedPosePublisher_; + rclcpp::Publisher::SharedPtr stateOptimizedPublisher_; + rclcpp::Publisher::SharedPtr stateOptimizedPosePublisher_; std::unique_ptr geometryVisualization_; }; diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_franka.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_franka.launch.py new file mode 100644 index 000000000..055a54bbd --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_franka.launch.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 + +"""Launch file for Franka mobile manipulator example.""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + # Package directories + ocs2_mobile_manipulator_ros_dir = get_package_share_directory('ocs2_mobile_manipulator_ros') + ocs2_robotic_assets_dir = get_package_share_directory('ocs2_robotic_assets') + ocs2_mobile_manipulator_dir = get_package_share_directory('ocs2_mobile_manipulator') + + # Launch arguments + rviz_arg = DeclareLaunchArgument( + 'rviz', + default_value='true', + description='Enable rviz visualization' + ) + + debug_arg = DeclareLaunchArgument( + 'debug', + default_value='false', + description='Set nodes on debug mode' + ) + + urdf_file_arg = DeclareLaunchArgument( + 'urdfFile', + default_value=os.path.join(ocs2_robotic_assets_dir, 'resources', 'mobile_manipulator', 'franka', 'urdf', 'panda.urdf'), + description='The URDF model of the robot' + ) + + task_file_arg = DeclareLaunchArgument( + 'taskFile', + default_value=os.path.join(ocs2_mobile_manipulator_dir, 'config', 'franka', 'task.info'), + description='The task file for the mpc' + ) + + lib_folder_arg = DeclareLaunchArgument( + 'libFolder', + default_value=os.path.join(ocs2_mobile_manipulator_dir, 'auto_generated', 'franka'), + description='The library folder to generate CppAD codegen into' + ) + + # Include main launch file + mobile_manipulator_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ocs2_mobile_manipulator_ros_dir, 'launch', 'mobile_manipulator.launch.py') + ), + launch_arguments={ + 'rviz': LaunchConfiguration('rviz'), + 'debug': LaunchConfiguration('debug'), + 'urdfFile': LaunchConfiguration('urdfFile'), + 'taskFile': LaunchConfiguration('taskFile'), + 'libFolder': LaunchConfiguration('libFolder'), + }.items() + ) + + return LaunchDescription([ + rviz_arg, + debug_arg, + urdf_file_arg, + task_file_arg, + lib_folder_arg, + mobile_manipulator_launch + ]) diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova.launch.py new file mode 100644 index 000000000..7843050ab --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova.launch.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python3 + +"""Launch file for Kinova mobile manipulator.""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + # Package directories + ocs2_mobile_manipulator_ros_dir = get_package_share_directory('ocs2_mobile_manipulator_ros') + + # Get file paths + urdf_file = os.path.join( + ocs2_mobile_manipulator_ros_dir, + 'urdf', + 'kinova_mobile_manipulator.urdf' + ) + + task_file = os.path.join( + ocs2_mobile_manipulator_ros_dir, + 'config', + 'mpc', + 'task.info' + ) + + lib_folder = '/tmp/ocs2' + + # Include main mobile manipulator launch file + mobile_manipulator_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ocs2_mobile_manipulator_ros_dir, 'launch', 'mobile_manipulator.launch.py') + ), + launch_arguments={ + 'urdfFile': urdf_file, + 'taskFile': task_file, + 'libFolder': lib_folder, + }.items() + ) + + return LaunchDescription([ + mobile_manipulator_launch + ]) diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_mabi_mobile.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_mabi_mobile.launch.py new file mode 100644 index 000000000..a3fc58bb0 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_mabi_mobile.launch.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python3 + +"""Launch file for MABI mobile manipulator.""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Package directories + ocs2_mobile_manipulator_ros_dir = get_package_share_directory('ocs2_mobile_manipulator_ros') + + # Launch arguments + rviz_arg = DeclareLaunchArgument( + 'rviz', + default_value='true', + description='Enable rviz visualization' + ) + + debug_arg = DeclareLaunchArgument( + 'debug', + default_value='false', + description='Set nodes on debug mode' + ) + + # Robot-specific file paths + urdf_file_arg = DeclareLaunchArgument( + 'urdfFile', + default_value=PathJoinSubstitution([ + FindPackageShare('ocs2_robotic_assets'), + 'resources', 'mobile_manipulator', 'mabi_mobile', 'urdf', 'mabi_mobile.urdf' + ]), + description='The URDF model of the robot' + ) + + task_file_arg = DeclareLaunchArgument( + 'taskFile', + default_value=PathJoinSubstitution([ + FindPackageShare('ocs2_mobile_manipulator'), + 'config', 'mabi_mobile', 'task.info' + ]), + description='The task file for the mpc' + ) + + lib_folder_arg = DeclareLaunchArgument( + 'libFolder', + default_value=PathJoinSubstitution([ + FindPackageShare('ocs2_mobile_manipulator'), + 'auto_generated', 'mabi_mobile' + ]), + description='The library folder to generate CppAD codegen into' + ) + + # Include main mobile manipulator launch file + mobile_manipulator_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ocs2_mobile_manipulator_ros_dir, 'launch', 'mobile_manipulator.launch.py') + ), + launch_arguments={ + 'rviz': LaunchConfiguration('rviz'), + 'debug': LaunchConfiguration('debug'), + 'urdfFile': LaunchConfiguration('urdfFile'), + 'taskFile': LaunchConfiguration('taskFile'), + 'libFolder': LaunchConfiguration('libFolder'), + }.items() + ) + + return LaunchDescription([ + rviz_arg, + debug_arg, + urdf_file_arg, + task_file_arg, + lib_folder_arg, + mobile_manipulator_launch + ]) diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_pr2.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_pr2.launch.py new file mode 100644 index 000000000..8d8f59220 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_pr2.launch.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python3 + +"""Launch file for PR2 mobile manipulator.""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + # Package directories + ocs2_mobile_manipulator_ros_dir = get_package_share_directory('ocs2_mobile_manipulator_ros') + + # Get file paths + urdf_file = os.path.join( + ocs2_mobile_manipulator_ros_dir, + 'urdf', + 'pr2_mobile_manipulator.urdf' + ) + + task_file = os.path.join( + ocs2_mobile_manipulator_ros_dir, + 'config', + 'mpc', + 'task.info' + ) + + lib_folder = '/tmp/ocs2' + + # Include main mobile manipulator launch file + mobile_manipulator_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ocs2_mobile_manipulator_ros_dir, 'launch', 'mobile_manipulator.launch.py') + ), + launch_arguments={ + 'urdfFile': urdf_file, + 'taskFile': task_file, + 'libFolder': lib_folder, + }.items() + ) + + return LaunchDescription([ + mobile_manipulator_launch + ]) diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_ridgeback_ur5.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_ridgeback_ur5.launch.py new file mode 100644 index 000000000..0bb99fcf8 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_ridgeback_ur5.launch.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python3 + +"""Launch file for Ridgeback UR5 mobile manipulator.""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Package directories + ocs2_mobile_manipulator_ros_dir = get_package_share_directory('ocs2_mobile_manipulator_ros') + + # Launch arguments + rviz_arg = DeclareLaunchArgument( + 'rviz', + default_value='true', + description='Enable rviz visualization' + ) + + debug_arg = DeclareLaunchArgument( + 'debug', + default_value='false', + description='Set nodes on debug mode' + ) + + # Robot-specific file paths + urdf_file_arg = DeclareLaunchArgument( + 'urdfFile', + default_value=PathJoinSubstitution([ + FindPackageShare('ocs2_robotic_assets'), + 'resources', 'mobile_manipulator', 'ridgeback_ur5', 'urdf', 'ridgeback_ur5.urdf' + ]), + description='The URDF model of the robot' + ) + + task_file_arg = DeclareLaunchArgument( + 'taskFile', + default_value=PathJoinSubstitution([ + FindPackageShare('ocs2_mobile_manipulator'), + 'config', 'ridgeback_ur5', 'task.info' + ]), + description='The task file for the mpc' + ) + + lib_folder_arg = DeclareLaunchArgument( + 'libFolder', + default_value=PathJoinSubstitution([ + FindPackageShare('ocs2_mobile_manipulator'), + 'auto_generated', 'ridgeback_ur5' + ]), + description='The library folder to generate CppAD codegen into' + ) + + # Include main mobile manipulator launch file + mobile_manipulator_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ocs2_mobile_manipulator_ros_dir, 'launch', 'mobile_manipulator.launch.py') + ), + launch_arguments={ + 'rviz': LaunchConfiguration('rviz'), + 'debug': LaunchConfiguration('debug'), + 'urdfFile': LaunchConfiguration('urdfFile'), + 'taskFile': LaunchConfiguration('taskFile'), + 'libFolder': LaunchConfiguration('libFolder'), + }.items() + ) + + return LaunchDescription([ + rviz_arg, + debug_arg, + urdf_file_arg, + task_file_arg, + lib_folder_arg, + mobile_manipulator_launch + ]) diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/mobile_manipulator.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/mobile_manipulator.launch.py new file mode 100644 index 000000000..59c709450 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/mobile_manipulator.launch.py @@ -0,0 +1,127 @@ +#!/usr/bin/env python3 + +"""Main launch file for mobile manipulator.""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess +from launch.conditions import IfCondition, UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import Node, SetParameter + + +def generate_launch_description(): + # Package directories + ocs2_mobile_manipulator_ros_dir = get_package_share_directory('ocs2_mobile_manipulator_ros') + + # Launch arguments + rviz_arg = DeclareLaunchArgument( + 'rviz', + default_value='true', + description='Enable rviz visualization' + ) + + debug_arg = DeclareLaunchArgument( + 'debug', + default_value='false', + description='Set nodes on debug mode' + ) + + urdf_file_arg = DeclareLaunchArgument( + 'urdfFile', + description='The URDF model of the robot' + ) + + task_file_arg = DeclareLaunchArgument( + 'taskFile', + description='The task file for the mpc' + ) + + lib_folder_arg = DeclareLaunchArgument( + 'libFolder', + description='The library folder to generate CppAD codegen into' + ) + + # Set parameters + set_task_file_param = SetParameter( + name='taskFile', + value=LaunchConfiguration('taskFile') + ) + + set_urdf_file_param = SetParameter( + name='urdfFile', + value=LaunchConfiguration('urdfFile') + ) + + set_lib_folder_param = SetParameter( + name='libFolder', + value=LaunchConfiguration('libFolder') + ) + + # Include visualization launch file + visualize_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ocs2_mobile_manipulator_ros_dir, 'launch', 'visualize.launch.py') + ), + launch_arguments={ + 'urdfFile': LaunchConfiguration('urdfFile'), + }.items(), + condition=IfCondition(LaunchConfiguration('rviz')) + ) + + # MPC node + mpc_node = Node( + package='ocs2_mobile_manipulator_ros', + executable='mobile_manipulator_mpc_node', + name='mobile_manipulator_mpc_node', + output='screen', + parameters=[ + {'taskFile': LaunchConfiguration('taskFile')}, + {'urdfFile': LaunchConfiguration('urdfFile')}, + {'libFolder': LaunchConfiguration('libFolder')}, + ] + ) + + # Dummy MRT node + dummy_mrt_node = Node( + package='ocs2_mobile_manipulator_ros', + executable='mobile_manipulator_dummy_mrt_node', + name='mobile_manipulator_dummy_mrt_node', + output='screen', + parameters=[ + {'taskFile': LaunchConfiguration('taskFile')}, + {'urdfFile': LaunchConfiguration('urdfFile')}, + {'libFolder': LaunchConfiguration('libFolder')}, + ] + ) + + # Target node (only when rviz is enabled) + target_node = Node( + package='ocs2_mobile_manipulator_ros', + executable='mobile_manipulator_target', + name='mobile_manipulator_target', + output='screen', + parameters=[ + {'taskFile': LaunchConfiguration('taskFile')}, + {'urdfFile': LaunchConfiguration('urdfFile')}, + {'libFolder': LaunchConfiguration('libFolder')}, + ], + condition=IfCondition(LaunchConfiguration('rviz')) + ) + + return LaunchDescription([ + rviz_arg, + debug_arg, + urdf_file_arg, + task_file_arg, + lib_folder_arg, + set_task_file_param, + set_urdf_file_param, + set_lib_folder_param, + visualize_launch, + mpc_node, + dummy_mrt_node, + target_node + ]) diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/visualize.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/visualize.launch.py new file mode 100644 index 000000000..78b48b67f --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/visualize.launch.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 + +"""Visualization launch file for mobile manipulator.""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # Package directories + ocs2_mobile_manipulator_ros_dir = get_package_share_directory('ocs2_mobile_manipulator_ros') + + # Launch arguments + urdf_file_arg = DeclareLaunchArgument( + 'urdfFile', + description='The URDF model of the robot' + ) + + # Robot state publisher node + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[ + {'robot_description': LaunchConfiguration('urdfFile')} + ] + ) + + # Joint state publisher node + joint_state_publisher = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + output='screen' + ) + + # RViz node + rviz_config = os.path.join( + ocs2_mobile_manipulator_ros_dir, + 'rviz', + 'mobile_manipulator.rviz' + ) + + rviz_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config] + ) + + return LaunchDescription([ + urdf_file_arg, + robot_state_publisher, + joint_state_publisher, + rviz_node + ]) diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/package.xml b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/package.xml index 3a68339d1..a4c6c2598 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/package.xml @@ -1,5 +1,5 @@ - + ocs2_mobile_manipulator_ros 0.0.0 The ocs2_mobile_manipulator_ros package @@ -11,11 +11,12 @@ BSD3 - catkin - cmake_clang_tools + ament_cmake - roslib - tf + rclcpp + tf2 + tf2_ros + tf2_geometry_msgs urdf kdl_parser robot_state_publisher @@ -32,5 +33,13 @@ ocs2_self_collision_visualization ocs2_mobile_manipulator pinocchio + hpp-fcl + + ament_lint_auto + ament_lint_common + + + ament_cmake + diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDistanceVisualization.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDistanceVisualization.cpp index 570a42606..f0592f890 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDistanceVisualization.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDistanceVisualization.cpp @@ -41,9 +41,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include -#include -#include +#include using namespace ocs2; using namespace mobile_manipulator; @@ -52,11 +50,11 @@ std::unique_ptr pInterface; std::shared_ptr gInterface; std::unique_ptr vInterface; -sensor_msgs::JointState lastMsg; +sensor_msgs::msg::JointState lastMsg; -std::unique_ptr pub; +rclcpp::Node::SharedPtr node; -void jointStateCallback(sensor_msgs::JointStateConstPtr msg) { +void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg) { if (lastMsg.position == msg->position) { return; } @@ -72,13 +70,19 @@ void jointStateCallback(sensor_msgs::JointStateConstPtr msg) { } int main(int argc, char** argv) { - // Initialize ros node - ros::init(argc, argv, "distance_visualization"); - ros::NodeHandle nodeHandle; + // Initialize ROS2 node + rclcpp::init(argc, argv); + node = rclcpp::Node::make_shared("distance_visualization", + rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)); + // Get ROS parameters std::string urdfPath, taskFile; - nodeHandle.getParam("/taskFile", taskFile); - nodeHandle.getParam("/urdfFile", urdfPath); + node->declare_parameter("taskFile", std::string("")); + node->declare_parameter("urdfFile", std::string("")); + taskFile = node->get_parameter("taskFile").as_string(); + urdfPath = node->get_parameter("urdfFile").as_string(); // read the task file boost::property_tree::ptree pt; @@ -120,11 +124,12 @@ int main(int argc, char** argv) { gInterface.reset(new PinocchioGeometryInterface(*pInterface, selfCollisionLinkPairs, selfCollisionObjectPairs)); - vInterface.reset(new GeometryInterfaceVisualization(*pInterface, *gInterface, nodeHandle, baseFrame)); + vInterface.reset(new GeometryInterfaceVisualization(node, *pInterface, *gInterface, baseFrame)); - ros::Subscriber sub = nodeHandle.subscribe("joint_states", 1, &jointStateCallback); + auto sub = node->create_subscription("joint_states", 1, &jointStateCallback); - ros::spin(); + rclcpp::spin(node); + rclcpp::shutdown(); return 0; } diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp index b472ae59b..22ea8b0e1 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp @@ -35,8 +35,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include -#include using namespace ocs2; using namespace mobile_manipulator; @@ -44,14 +42,21 @@ using namespace mobile_manipulator; int main(int argc, char** argv) { const std::string robotName = "mobile_manipulator"; - // Initialize ros node - ros::init(argc, argv, robotName + "_mrt"); - ros::NodeHandle nodeHandle; + // Initialize ROS2 node + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared(robotName + "_mrt", + rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)); + // Get node parameters std::string taskFile, libFolder, urdfFile; - nodeHandle.getParam("/taskFile", taskFile); - nodeHandle.getParam("/libFolder", libFolder); - nodeHandle.getParam("/urdfFile", urdfFile); + node->declare_parameter("taskFile", std::string("")); + node->declare_parameter("libFolder", std::string("")); + node->declare_parameter("urdfFile", std::string("")); + taskFile = node->get_parameter("taskFile").as_string(); + libFolder = node->get_parameter("libFolder").as_string(); + urdfFile = node->get_parameter("urdfFile").as_string(); std::cerr << "Loading task file: " << taskFile << std::endl; std::cerr << "Loading library folder: " << libFolder << std::endl; std::cerr << "Loading urdf file: " << urdfFile << std::endl; @@ -61,10 +66,10 @@ int main(int argc, char** argv) { // MRT MRT_ROS_Interface mrt(robotName); mrt.initRollout(&interface.getRollout()); - mrt.launchNodes(nodeHandle); + mrt.launchNodes(node); // Visualization - auto dummyVisualization = std::make_shared(nodeHandle, interface); + auto dummyVisualization = std::make_shared(node, interface); // Dummy MRT MRT_ROS_Dummy_Loop dummy(mrt, interface.mpcSettings().mrtDesiredFrequency_, interface.mpcSettings().mpcDesiredFrequency_); @@ -86,6 +91,9 @@ int main(int argc, char** argv) { // Run dummy (loops while ros is ok) dummy.run(initObservation, initTargetTrajectories); + // ROS2 cleanup + rclcpp::shutdown(); + // Successful exit return 0; } diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyVisualization.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyVisualization.cpp index 9a1d6dcee..cb807bf1b 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyVisualization.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyVisualization.cpp @@ -32,13 +32,16 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include -#include +#include +#include +#include + #include #include -#include -#include +#include +#include +#include #include #include @@ -57,7 +60,7 @@ namespace mobile_manipulator { /******************************************************************************************************/ /******************************************************************************************************/ template -void assignHeader(It firstIt, It lastIt, const std_msgs::Header& header) { +void assignHeader(It firstIt, It lastIt, const std_msgs::msg::Header& header) { for (; firstIt != lastIt; ++firstIt) { firstIt->header = header; } @@ -76,27 +79,43 @@ void assignIncreasingId(It firstIt, It lastIt, int startId = 0) { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MobileManipulatorDummyVisualization::launchVisualizerNode(ros::NodeHandle& nodeHandle) { +void MobileManipulatorDummyVisualization::launchVisualizerNode(rclcpp::Node::SharedPtr node) { // load a kdl-tree from the urdf robot description and initialize the robot state publisher const std::string urdfName = "robot_description"; + std::string urdfString; urdf::Model model; - if (!model.initParam(urdfName)) { - ROS_ERROR("URDF model load was NOT successful"); + + // In ROS2, use get_parameter instead of initParam + if (!node->get_parameter(urdfName, urdfString)) { + RCLCPP_ERROR(node->get_logger(), "Failed to get URDF from parameter: %s", urdfName.c_str()); + return; + } + + // Use initString instead of initParam for ROS2 + if (!model.initString(urdfString)) { + RCLCPP_ERROR(node->get_logger(), "Failed to parse URDF string"); + return; } + KDL::Tree tree; if (!kdl_parser::treeFromUrdfModel(model, tree)) { - ROS_ERROR("Failed to extract kdl tree from xml robot description"); + RCLCPP_ERROR(node->get_logger(), "Failed to extract kdl tree from xml robot description"); } - robotStatePublisherPtr_.reset(new robot_state_publisher::RobotStatePublisher(tree)); - robotStatePublisherPtr_->publishFixedTransforms(true); + node->declare_parameter("publish_fixed_transforms", true); + jointStatePublisher_ = node->create_publisher("joint_states", 10); + // Initialize tf2 broadcaster + tfBroadcaster_ = std::make_unique(node); + + stateOptimizedPublisher_ = node->create_publisher("/mobile_manipulator/optimizedStateTrajectory", 1); + stateOptimizedPosePublisher_ = node->create_publisher("/mobile_manipulator/optimizedPoseTrajectory", 1); - stateOptimizedPublisher_ = nodeHandle.advertise("/mobile_manipulator/optimizedStateTrajectory", 1); - stateOptimizedPosePublisher_ = nodeHandle.advertise("/mobile_manipulator/optimizedPoseTrajectory", 1); // Get ROS parameter std::string urdfFile, taskFile; - nodeHandle.getParam("/urdfFile", urdfFile); - nodeHandle.getParam("/taskFile", taskFile); + node->declare_parameter("urdfFile", std::string("")); + node->declare_parameter("taskFile", std::string("")); + urdfFile = node->get_parameter("urdfFile").as_string(); + taskFile = node->get_parameter("taskFile").as_string(); // read manipulator type ManipulatorModelType modelType = mobile_manipulator::loadManipulatorType(taskFile, "model_information.manipulatorModelType"); // read the joints to make fixed @@ -114,7 +133,7 @@ void MobileManipulatorDummyVisualization::launchVisualizerNode(ros::NodeHandle& loadData::loadStdVectorOfPair(taskFile, "selfCollision.collisionObjectPairs", collisionObjectPairs, true); PinocchioGeometryInterface geomInterface(pinocchioInterface, collisionObjectPairs); // set geometry visualization markers - geometryVisualization_.reset(new GeometryInterfaceVisualization(std::move(pinocchioInterface), geomInterface, nodeHandle)); + geometryVisualization_.reset(new GeometryInterfaceVisualization(node, std::move(pinocchioInterface), geomInterface)); } } @@ -123,7 +142,7 @@ void MobileManipulatorDummyVisualization::launchVisualizerNode(ros::NodeHandle& /******************************************************************************************************/ void MobileManipulatorDummyVisualization::update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) { - const ros::Time timeStamp = ros::Time::now(); + const rclcpp::Time timeStamp = node_->now(); publishObservation(timeStamp, observation); publishTargetTrajectories(timeStamp, command.mpcTargetTrajectories_); @@ -136,71 +155,83 @@ void MobileManipulatorDummyVisualization::update(const SystemObservation& observ /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MobileManipulatorDummyVisualization::publishObservation(const ros::Time& timeStamp, const SystemObservation& observation) { +void MobileManipulatorDummyVisualization::publishObservation(const rclcpp::Time& timeStamp, const SystemObservation& observation) { // publish world -> base transform const auto r_world_base = getBasePosition(observation.state, modelInfo_); const Eigen::Quaternion q_world_base = getBaseOrientation(observation.state, modelInfo_); - geometry_msgs::TransformStamped base_tf; + geometry_msgs::msg::TransformStamped base_tf; base_tf.header.stamp = timeStamp; base_tf.header.frame_id = "world"; base_tf.child_frame_id = modelInfo_.baseFrame; base_tf.transform.translation = ros_msg_helpers::getVectorMsg(r_world_base); base_tf.transform.rotation = ros_msg_helpers::getOrientationMsg(q_world_base); - tfBroadcaster_.sendTransform(base_tf); + tfBroadcaster_->sendTransform(base_tf); - // publish joints transforms const auto j_arm = getArmJointAngles(observation.state, modelInfo_); std::map jointPositions; for (size_t i = 0; i < modelInfo_.dofNames.size(); i++) { jointPositions[modelInfo_.dofNames[i]] = j_arm(i); } - for (const auto& name : removeJointNames_) { - jointPositions[name] = 0.0; + // publish joints transforms + // + + sensor_msgs::msg::JointState jointState; + jointState.header.stamp = timeStamp; + jointState.header.frame_id = ""; + + // fill joint name and position + for (const auto& joint : jointPositions) { + jointState.name.push_back(joint.first); + jointState.position.push_back(joint.second); + } + + // pub JointState and let robot_state_publisher handle transform + if (jointStatePublisher_) { + jointStatePublisher_->publish(jointState); } - robotStatePublisherPtr_->publishTransforms(jointPositions, timeStamp); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MobileManipulatorDummyVisualization::publishTargetTrajectories(const ros::Time& timeStamp, +void MobileManipulatorDummyVisualization::publishTargetTrajectories(const rclcpp::Time& timeStamp, const TargetTrajectories& targetTrajectories) { // publish command transform const Eigen::Vector3d eeDesiredPosition = targetTrajectories.stateTrajectory.back().head(3); Eigen::Quaterniond eeDesiredOrientation; eeDesiredOrientation.coeffs() = targetTrajectories.stateTrajectory.back().tail(4); - geometry_msgs::TransformStamped command_tf; + geometry_msgs::msg::TransformStamped command_tf; command_tf.header.stamp = timeStamp; command_tf.header.frame_id = "world"; command_tf.child_frame_id = "command"; command_tf.transform.translation = ros_msg_helpers::getVectorMsg(eeDesiredPosition); command_tf.transform.rotation = ros_msg_helpers::getOrientationMsg(eeDesiredOrientation); - tfBroadcaster_.sendTransform(command_tf); + tfBroadcaster_->sendTransform(command_tf); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MobileManipulatorDummyVisualization::publishOptimizedTrajectory(const ros::Time& timeStamp, const PrimalSolution& policy) { +void MobileManipulatorDummyVisualization::publishOptimizedTrajectory(const rclcpp::Time& timeStamp, const PrimalSolution& policy) { const scalar_t TRAJECTORYLINEWIDTH = 0.005; const std::array red{0.6350, 0.0780, 0.1840}; const std::array blue{0, 0.4470, 0.7410}; const auto& mpcStateTrajectory = policy.stateTrajectory_; - visualization_msgs::MarkerArray markerArray; + visualization_msgs::msg::MarkerArray markerArray; // Base trajectory - std::vector baseTrajectory; + std::vector baseTrajectory; baseTrajectory.reserve(mpcStateTrajectory.size()); - geometry_msgs::PoseArray poseArray; + geometry_msgs::msg::PoseArray poseArray; poseArray.poses.reserve(mpcStateTrajectory.size()); // End effector trajectory const auto& model = pinocchioInterface_.getModel(); auto& data = pinocchioInterface_.getData(); - std::vector endEffectorTrajectory; + std::vector endEffectorTrajectory; endEffectorTrajectory.reserve(mpcStateTrajectory.size()); std::for_each(mpcStateTrajectory.begin(), mpcStateTrajectory.end(), [&](const Eigen::VectorXd& state) { pinocchio::forwardKinematics(model, data, state); @@ -220,7 +251,7 @@ void MobileManipulatorDummyVisualization::publishOptimizedTrajectory(const ros:: const Eigen::Quaternion q_world_base = getBaseOrientation(state, modelInfo_); // convert to ros message - geometry_msgs::Pose pose; + geometry_msgs::msg::Pose pose; pose.position = ros_msg_helpers::getPointMsg(r_world_base); pose.orientation = ros_msg_helpers::getOrientationMsg(q_world_base); baseTrajectory.push_back(pose.position); @@ -234,8 +265,8 @@ void MobileManipulatorDummyVisualization::publishOptimizedTrajectory(const ros:: assignIncreasingId(markerArray.markers.begin(), markerArray.markers.end()); poseArray.header = ros_msg_helpers::getHeaderMsg("world", timeStamp); - stateOptimizedPublisher_.publish(markerArray); - stateOptimizedPosePublisher_.publish(poseArray); + stateOptimizedPublisher_->publish(markerArray); + stateOptimizedPosePublisher_->publish(poseArray); } } // namespace mobile_manipulator diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp index 64ebb951f..5b97cbc71 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp @@ -27,8 +27,6 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include -#include #include #include @@ -42,14 +40,21 @@ using namespace mobile_manipulator; int main(int argc, char** argv) { const std::string robotName = "mobile_manipulator"; - // Initialize ros node - ros::init(argc, argv, robotName + "_mpc"); - ros::NodeHandle nodeHandle; + // Initialize ROS2 node + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared(robotName + "_mpc", + rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)); + // Get node parameters std::string taskFile, libFolder, urdfFile; - nodeHandle.getParam("/taskFile", taskFile); - nodeHandle.getParam("/libFolder", libFolder); - nodeHandle.getParam("/urdfFile", urdfFile); + node->declare_parameter("taskFile", std::string("")); + node->declare_parameter("libFolder", std::string("")); + node->declare_parameter("urdfFile", std::string("")); + taskFile = node->get_parameter("taskFile").as_string(); + libFolder = node->get_parameter("libFolder").as_string(); + urdfFile = node->get_parameter("urdfFile").as_string(); std::cerr << "Loading task file: " << taskFile << std::endl; std::cerr << "Loading library folder: " << libFolder << std::endl; std::cerr << "Loading urdf file: " << urdfFile << std::endl; @@ -58,7 +63,7 @@ int main(int argc, char** argv) { // ROS ReferenceManager auto rosReferenceManagerPtr = std::make_shared(robotName, interface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); + rosReferenceManagerPtr->subscribe(node); // MPC ocs2::GaussNewtonDDP_MPC mpc(interface.mpcSettings(), interface.ddpSettings(), interface.getRollout(), @@ -67,7 +72,9 @@ int main(int argc, char** argv) { // Launch MPC ROS node MPC_ROS_Interface mpcNode(mpc, robotName); - mpcNode.launchNodes(nodeHandle); + mpcNode.launchNodes(node); + + rclcpp::shutdown(); // Successful exit return 0; diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorTarget.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorTarget.cpp index 371d16958..6691e5065 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorTarget.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorTarget.cpp @@ -27,6 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ +#include #include using namespace ocs2; @@ -49,12 +50,18 @@ TargetTrajectories goalPoseToTargetTrajectories(const Eigen::Vector3d& position, int main(int argc, char* argv[]) { const std::string robotName = "mobile_manipulator"; - ::ros::init(argc, argv, robotName + "_target"); - ::ros::NodeHandle nodeHandle; - TargetTrajectoriesInteractiveMarker targetPoseCommand(nodeHandle, robotName, &goalPoseToTargetTrajectories); + // Initialize ROS2 node + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared(robotName + "_target"); + + TargetTrajectoriesInteractiveMarker targetPoseCommand(node, robotName, &goalPoseToTargetTrajectories); targetPoseCommand.publishInteractiveMarker(); + // ROS2 spin + rclcpp::spin(node); + rclcpp::shutdown(); + // Successful exit return 0; } From ea36d1049b717604d341cac651d5d11558adfff6 Mon Sep 17 00:00:00 2001 From: "Yu, Yan" Date: Fri, 12 Sep 2025 17:08:19 +0800 Subject: [PATCH 39/42] Fix Franka example launch (#52) --- .../launch/visualize.launch.py | 16 +- .../rviz/mobile_manipulator.rviz | 197 +++++++++--------- .../src/MobileManipulatorDummyMRT.cpp | 6 +- .../MobileManipulatorDummyVisualization.cpp | 28 +-- .../src/MobileManipulatorMpcNode.cpp | 6 +- 5 files changed, 105 insertions(+), 148 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/visualize.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/visualize.launch.py index 78b48b67f..710ced6aa 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/visualize.launch.py +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/visualize.launch.py @@ -17,7 +17,8 @@ def generate_launch_description(): # Launch arguments urdf_file_arg = DeclareLaunchArgument( 'urdfFile', - description='The URDF model of the robot' + description='The URDF model of the robot', + default_value=get_package_share_directory('ocs2_robotic_assets') + "/resources/mobile_manipulator/franka/urdf/panda.urdf" ) # Robot state publisher node @@ -26,17 +27,7 @@ def generate_launch_description(): executable='robot_state_publisher', name='robot_state_publisher', output='screen', - parameters=[ - {'robot_description': LaunchConfiguration('urdfFile')} - ] - ) - - # Joint state publisher node - joint_state_publisher = Node( - package='joint_state_publisher', - executable='joint_state_publisher', - name='joint_state_publisher', - output='screen' + arguments=[LaunchConfiguration("urdfFile")] ) # RViz node @@ -57,6 +48,5 @@ def generate_launch_description(): return LaunchDescription([ urdf_file_arg, robot_state_publisher, - joint_state_publisher, rviz_node ]) diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/rviz/mobile_manipulator.rviz b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/rviz/mobile_manipulator.rviz index 2de59a852..c327014c1 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/rviz/mobile_manipulator.rviz +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/rviz/mobile_manipulator.rviz @@ -1,41 +1,38 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 + - /MarkerArray1 + - /InteractiveMarkers1/Status1 Splitter Ratio: 0.5 - Tree Height: 555 - - Class: rviz/Selection + Tree Height: 445 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -52,8 +49,16 @@ Visualization Manager: Reference Frame: Value: true - Alpha: 0.699999988079071 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description Enabled: true Links: ARM: @@ -104,161 +109,145 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false + Mass Properties: + Inertia: false + Mass: false Name: RobotModel - Robot Description: robot_description TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true - - Class: rviz/TF - Enabled: true + - Class: rviz_default_plugins/TF + Enabled: false Frame Timeout: 15 Frames: - ARM: - Value: true All Enabled: true - ELBOW: - Value: true - FOREARM: - Value: true - SHOULDER: - Value: true - WRIST_1: - Value: true - WRIST_2: - Value: true - arm_base: - Value: true - arm_base_inertia: - Value: true - base: - Value: true - command: - Value: true - world: - Value: true - Marker Alpha: 1 Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: - world: - base: - arm_base: - SHOULDER: - ARM: - ELBOW: - FOREARM: - WRIST_1: - WRIST_2: - {} - arm_base_inertia: - {} - command: - {} + {} Update Interval: 0 - Value: true - - Class: rviz/MarkerArray + Value: false + - Class: rviz_default_plugins/MarkerArray Enabled: true - Marker Topic: /mobile_manipulator/optimizedStateTrajectory Name: MarkerArray Namespaces: - Base Trajectory: true - EE Trajectory: true - Queue Size: 100 + 1 - 4: true + 1 - 6: true + 1 - 9: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /distance_markers Value: true - Alpha: 1 Arrow Length: 0.30000001192092896 Axes Length: 0.10000000149011612 Axes Radius: 0.009999999776482582 - Class: rviz/PoseArray + Class: rviz_default_plugins/PoseArray Color: 255; 25; 0 Enabled: true Head Length: 0.07000000029802322 Head Radius: 0.029999999329447746 Name: PoseArray - Queue Size: 10 Shaft Length: 0.23000000417232513 Shaft Radius: 0.009999999776482582 Shape: Axes - Topic: /mobile_manipulator/optimizedPoseTrajectory - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /mobile_manipulator/optimizedPoseTrajectory Value: true - - Class: rviz/InteractiveMarkers + - Class: rviz_default_plugins/InteractiveMarkers Enable Transparency: true Enabled: true + Interactive Markers Namespace: simple_marker Name: InteractiveMarkers - Show Axes: false + Show Axes: true Show Descriptions: true Show Visual Aids: false - Update Topic: /simple_marker/update - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /distance_markers - Name: MarkerArray - Namespaces: - 1 - 4: true - 1 - 6: true - Queue Size: 100 Value: true Enabled: true Global Options: - Background Color: 186; 189; 182 - Default Light: true + Background Color: 255; 255; 255 Fixed Frame: world Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit - Distance: 5.996953964233398 + Class: rviz_default_plugins/Orbit + Distance: 3.3964345455169678 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Field of View: 0.7853981852531433 Focal Point: - X: 0 - Y: 0 - Z: 0 + X: 0.1456933319568634 + Y: -0.3809991180896759 + Z: 0.32674264907836914 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.4003985524177551 + Pitch: 0.24539874494075775 Target Frame: - Yaw: 0.44039851427078247 + Value: Orbit (rviz_default_plugins) + Yaw: 0.8853984475135803 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 846 + Height: 736 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000015600000246fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000246000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004ba0000003efc0100000002fb0000000800540069006d00650100000000000004ba0000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000035e0000024600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -267,6 +256,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1200 - X: 720 - Y: 280 + Width: 1210 + X: 851 + Y: 122 diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp index 22ea8b0e1..33d3a7027 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp @@ -51,9 +51,9 @@ int main(int argc, char** argv) { // Get node parameters std::string taskFile, libFolder, urdfFile; - node->declare_parameter("taskFile", std::string("")); - node->declare_parameter("libFolder", std::string("")); - node->declare_parameter("urdfFile", std::string("")); + // node->declare_parameter("taskFile", std::string("")); + // node->declare_parameter("libFolder", std::string("")); + // node->declare_parameter("urdfFile", std::string("")); taskFile = node->get_parameter("taskFile").as_string(); libFolder = node->get_parameter("libFolder").as_string(); urdfFile = node->get_parameter("urdfFile").as_string(); diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyVisualization.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyVisualization.cpp index cb807bf1b..69d0476c8 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyVisualization.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyVisualization.cpp @@ -80,29 +80,7 @@ void assignIncreasingId(It firstIt, It lastIt, int startId = 0) { /******************************************************************************************************/ /******************************************************************************************************/ void MobileManipulatorDummyVisualization::launchVisualizerNode(rclcpp::Node::SharedPtr node) { - // load a kdl-tree from the urdf robot description and initialize the robot state publisher - const std::string urdfName = "robot_description"; - std::string urdfString; - urdf::Model model; - - // In ROS2, use get_parameter instead of initParam - if (!node->get_parameter(urdfName, urdfString)) { - RCLCPP_ERROR(node->get_logger(), "Failed to get URDF from parameter: %s", urdfName.c_str()); - return; - } - - // Use initString instead of initParam for ROS2 - if (!model.initString(urdfString)) { - RCLCPP_ERROR(node->get_logger(), "Failed to parse URDF string"); - return; - } - - KDL::Tree tree; - if (!kdl_parser::treeFromUrdfModel(model, tree)) { - RCLCPP_ERROR(node->get_logger(), "Failed to extract kdl tree from xml robot description"); - } - - node->declare_parameter("publish_fixed_transforms", true); + jointStatePublisher_ = node->create_publisher("joint_states", 10); // Initialize tf2 broadcaster tfBroadcaster_ = std::make_unique(node); @@ -112,8 +90,8 @@ void MobileManipulatorDummyVisualization::launchVisualizerNode(rclcpp::Node::Sha // Get ROS parameter std::string urdfFile, taskFile; - node->declare_parameter("urdfFile", std::string("")); - node->declare_parameter("taskFile", std::string("")); + // node->declare_parameter("urdfFile", std::string("")); + // node->declare_parameter("taskFile", std::string("")); urdfFile = node->get_parameter("urdfFile").as_string(); taskFile = node->get_parameter("taskFile").as_string(); // read manipulator type diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp index 5b97cbc71..7dfc58d76 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp @@ -49,9 +49,9 @@ int main(int argc, char** argv) { // Get node parameters std::string taskFile, libFolder, urdfFile; - node->declare_parameter("taskFile", std::string("")); - node->declare_parameter("libFolder", std::string("")); - node->declare_parameter("urdfFile", std::string("")); + // node->declare_parameter("taskFile", std::string("")); + // node->declare_parameter("libFolder", std::string("")); + // node->declare_parameter("urdfFile", std::string("")); taskFile = node->get_parameter("taskFile").as_string(); libFolder = node->get_parameter("libFolder").as_string(); urdfFile = node->get_parameter("urdfFile").as_string(); From cc1339afb9317db4d79f157bcb37252624474daa Mon Sep 17 00:00:00 2001 From: jzhao6x <88374403+jzhao6x@users.noreply.github.com> Date: Thu, 25 Sep 2025 10:47:33 +0800 Subject: [PATCH 40/42] fix missing pkg for ocs2 examples running (#53) Co-authored-by: RoboticsYY --- .docker/Dockerfile | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.docker/Dockerfile b/.docker/Dockerfile index f939ca94e..aef2ce10b 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -19,7 +19,8 @@ RUN apt-get install -y \ RUN apt-get install -y \ ros-humble-pinocchio \ - ros-humble-hpp-fcl + ros-humble-hpp-fcl \ + ros-humble-joint-state-publisher RUN apt-get clean \ && rm -rf /var/lib/apt/lists/* From 0849cb465e657221c74b845206c6adf4931c1159 Mon Sep 17 00:00:00 2001 From: "Yu, Yan" Date: Thu, 25 Sep 2025 15:50:41 +0800 Subject: [PATCH 41/42] Fix mobile manipulator example issues (#54) * Enable kinova and pr2 example runable * Fix visualization issue for removed joints --- .github/CODEOWNERS | 26 ++++--- .../ocs2_mobile_manipulator/package.xml | 5 +- .../launch/manipulator_kinova.launch.py | 46 ------------ .../launch/manipulator_kinova_j2n6.launch.py | 71 +++++++++++++++++++ .../launch/manipulator_kinova_j2n7.launch.py | 71 +++++++++++++++++++ .../launch/manipulator_pr2.launch.py | 57 ++++++++++----- .../MobileManipulatorDummyVisualization.cpp | 7 +- ocs2_ros_interfaces/package.xml | 2 - 8 files changed, 207 insertions(+), 78 deletions(-) delete mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova.launch.py create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n6.launch.py create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n7.launch.py diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index fae89ff91..9403c4e68 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -3,11 +3,21 @@ # Default Reviewers: * @RoboticsYY @baihe-liu @AnnikaWU -# code owner -/.docker @jzhao6x -/.github @jzhao6x -/ocs2_thirdpary @xzhan34 -/ocs2_msgs @sunausti -/ocs2_robotic_tools @sunausti -/ocs2_core @xzhan34 -/ocs2_oc @xzhan34 +# OCS2 ROS2 porting code owner +/.docker @jzhao6x +/.github @jzhao6x +/ocs2_thirdpary @xzhan34 +/ocs2_msgs @sunausti +/ocs2_robotic_tools @sunausti +/ocs2_core @xzhan34 +/ocs2_oc @xzhan34 +/ocs2_test_tools/ocs2_qp_solver @Nisyhaal +/ocs2_mpc @baihe-liu +/ocs2_robotic_tools @sunausti +/ocs2_ddp @sunausti +/ocs2_ros_interface @xzhan34 +/ocs2_pinocchio/ocs2_pinocchio_interface @sunausti +/ocs2_pinocchio/ocs2_self_collision @sunausti +/ocs2_pinocchio/ocs2_self_collision_visualization @Nisyhaal +/ocs2_robotic_examples/ocs2_mobile_manipulator @baihe-liu +/ocs2_robotic_examples/ocs2_mobile_manipulator_ros @sunausti diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/package.xml b/ocs2_robotic_examples/ocs2_mobile_manipulator/package.xml index 930de6a84..72908dc91 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/package.xml +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/package.xml @@ -14,8 +14,6 @@ eigen3_cmake_module eigen3_cmake_module - cmake_clang_tools - ocs2_core ocs2_ddp ocs2_mpc @@ -25,7 +23,6 @@ ocs2_self_collision pinocchio eigen - Boost urdf urdfdom hpp-fcl @@ -35,4 +32,4 @@ ament_cmake - \ No newline at end of file + diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova.launch.py deleted file mode 100644 index 7843050ab..000000000 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova.launch.py +++ /dev/null @@ -1,46 +0,0 @@ -#!/usr/bin/env python3 - -"""Launch file for Kinova mobile manipulator.""" - -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource - - -def generate_launch_description(): - # Package directories - ocs2_mobile_manipulator_ros_dir = get_package_share_directory('ocs2_mobile_manipulator_ros') - - # Get file paths - urdf_file = os.path.join( - ocs2_mobile_manipulator_ros_dir, - 'urdf', - 'kinova_mobile_manipulator.urdf' - ) - - task_file = os.path.join( - ocs2_mobile_manipulator_ros_dir, - 'config', - 'mpc', - 'task.info' - ) - - lib_folder = '/tmp/ocs2' - - # Include main mobile manipulator launch file - mobile_manipulator_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(ocs2_mobile_manipulator_ros_dir, 'launch', 'mobile_manipulator.launch.py') - ), - launch_arguments={ - 'urdfFile': urdf_file, - 'taskFile': task_file, - 'libFolder': lib_folder, - }.items() - ) - - return LaunchDescription([ - mobile_manipulator_launch - ]) diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n6.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n6.launch.py new file mode 100644 index 000000000..3b8704e3b --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n6.launch.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 + +"""Launch file for Kinova mobile manipulator.""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + # Package directories + ocs2_mobile_manipulator_ros_dir = get_package_share_directory('ocs2_mobile_manipulator_ros') + ocs2_robotic_assets_dir = get_package_share_directory('ocs2_robotic_assets') + ocs2_mobile_manipulator_dir = get_package_share_directory('ocs2_mobile_manipulator') + + # Launch arguments + rviz_arg = DeclareLaunchArgument( + 'rviz', + default_value='true', + description='Enable rviz visualization' + ) + + debug_arg = DeclareLaunchArgument( + 'debug', + default_value='false', + description='Set nodes on debug mode' + ) + + urdf_file_arg = DeclareLaunchArgument( + 'urdfFile', + default_value=os.path.join(ocs2_robotic_assets_dir, 'resources', 'mobile_manipulator', 'kinova', 'urdf', 'j2n6s300.urdf'), + description='The URDF model of the robot' + ) + + task_file_arg = DeclareLaunchArgument( + 'taskFile', + default_value=os.path.join(ocs2_mobile_manipulator_dir, 'config', 'kinova', 'task_j2n6.info'), + description='The task file for the mpc' + ) + + lib_folder_arg = DeclareLaunchArgument( + 'libFolder', + default_value=os.path.join(ocs2_mobile_manipulator_dir, 'auto_generated', 'kinova', 'j2n6'), + description='The library folder to generate CppAD codegen into' + ) + + # Include main launch file + mobile_manipulator_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ocs2_mobile_manipulator_ros_dir, 'launch', 'mobile_manipulator.launch.py') + ), + launch_arguments={ + 'rviz': LaunchConfiguration('rviz'), + 'debug': LaunchConfiguration('debug'), + 'urdfFile': LaunchConfiguration('urdfFile'), + 'taskFile': LaunchConfiguration('taskFile'), + 'libFolder': LaunchConfiguration('libFolder'), + }.items() + ) + + return LaunchDescription([ + rviz_arg, + debug_arg, + urdf_file_arg, + task_file_arg, + lib_folder_arg, + mobile_manipulator_launch + ]) \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n7.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n7.launch.py new file mode 100644 index 000000000..be7985654 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_kinova_j2n7.launch.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 + +"""Launch file for Kinova mobile manipulator.""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + # Package directories + ocs2_mobile_manipulator_ros_dir = get_package_share_directory('ocs2_mobile_manipulator_ros') + ocs2_robotic_assets_dir = get_package_share_directory('ocs2_robotic_assets') + ocs2_mobile_manipulator_dir = get_package_share_directory('ocs2_mobile_manipulator') + + # Launch arguments + rviz_arg = DeclareLaunchArgument( + 'rviz', + default_value='true', + description='Enable rviz visualization' + ) + + debug_arg = DeclareLaunchArgument( + 'debug', + default_value='false', + description='Set nodes on debug mode' + ) + + urdf_file_arg = DeclareLaunchArgument( + 'urdfFile', + default_value=os.path.join(ocs2_robotic_assets_dir, 'resources', 'mobile_manipulator', 'kinova', 'urdf', 'j2n7s300.urdf'), + description='The URDF model of the robot' + ) + + task_file_arg = DeclareLaunchArgument( + 'taskFile', + default_value=os.path.join(ocs2_mobile_manipulator_dir, 'config', 'kinova', 'task_j2n7.info'), + description='The task file for the mpc' + ) + + lib_folder_arg = DeclareLaunchArgument( + 'libFolder', + default_value=os.path.join(ocs2_mobile_manipulator_dir, 'auto_generated', 'kinova', 'j2n7'), + description='The library folder to generate CppAD codegen into' + ) + + # Include main launch file + mobile_manipulator_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ocs2_mobile_manipulator_ros_dir, 'launch', 'mobile_manipulator.launch.py') + ), + launch_arguments={ + 'rviz': LaunchConfiguration('rviz'), + 'debug': LaunchConfiguration('debug'), + 'urdfFile': LaunchConfiguration('urdfFile'), + 'taskFile': LaunchConfiguration('taskFile'), + 'libFolder': LaunchConfiguration('libFolder'), + }.items() + ) + + return LaunchDescription([ + rviz_arg, + debug_arg, + urdf_file_arg, + task_file_arg, + lib_folder_arg, + mobile_manipulator_launch + ]) diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_pr2.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_pr2.launch.py index 8d8f59220..2de6efa7b 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_pr2.launch.py +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_pr2.launch.py @@ -5,42 +5,67 @@ import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration def generate_launch_description(): # Package directories ocs2_mobile_manipulator_ros_dir = get_package_share_directory('ocs2_mobile_manipulator_ros') + ocs2_robotic_assets_dir = get_package_share_directory('ocs2_robotic_assets') + ocs2_mobile_manipulator_dir = get_package_share_directory('ocs2_mobile_manipulator') - # Get file paths - urdf_file = os.path.join( - ocs2_mobile_manipulator_ros_dir, - 'urdf', - 'pr2_mobile_manipulator.urdf' + # Launch arguments + rviz_arg = DeclareLaunchArgument( + 'rviz', + default_value='true', + description='Enable rviz visualization' ) - task_file = os.path.join( - ocs2_mobile_manipulator_ros_dir, - 'config', - 'mpc', - 'task.info' + debug_arg = DeclareLaunchArgument( + 'debug', + default_value='false', + description='Set nodes on debug mode' ) - lib_folder = '/tmp/ocs2' + urdf_file_arg = DeclareLaunchArgument( + 'urdfFile', + default_value=os.path.join(ocs2_robotic_assets_dir, 'resources', 'mobile_manipulator', 'pr2', 'urdf', 'pr2.urdf'), + description='The URDF model of the robot' + ) + + task_file_arg = DeclareLaunchArgument( + 'taskFile', + default_value=os.path.join(ocs2_mobile_manipulator_dir, 'config', 'pr2', 'task.info'), + description='The task file for the mpc' + ) + + lib_folder_arg = DeclareLaunchArgument( + 'libFolder', + default_value=os.path.join(ocs2_mobile_manipulator_dir, 'auto_generated', 'pr2'), + description='The library folder to generate CppAD codegen into' + ) - # Include main mobile manipulator launch file + # Include main launch file mobile_manipulator_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(ocs2_mobile_manipulator_ros_dir, 'launch', 'mobile_manipulator.launch.py') ), launch_arguments={ - 'urdfFile': urdf_file, - 'taskFile': task_file, - 'libFolder': lib_folder, + 'rviz': LaunchConfiguration('rviz'), + 'debug': LaunchConfiguration('debug'), + 'urdfFile': LaunchConfiguration('urdfFile'), + 'taskFile': LaunchConfiguration('taskFile'), + 'libFolder': LaunchConfiguration('libFolder'), }.items() ) return LaunchDescription([ + rviz_arg, + debug_arg, + urdf_file_arg, + task_file_arg, + lib_folder_arg, mobile_manipulator_launch ]) diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyVisualization.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyVisualization.cpp index 69d0476c8..5f4a0056e 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyVisualization.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyVisualization.cpp @@ -151,9 +151,12 @@ void MobileManipulatorDummyVisualization::publishObservation(const rclcpp::Time& for (size_t i = 0; i < modelInfo_.dofNames.size(); i++) { jointPositions[modelInfo_.dofNames[i]] = j_arm(i); } - // publish joints transforms - // + for (const auto& name : removeJointNames_) { + jointPositions[name] = 0.0; + } + + // publish joints transforms sensor_msgs::msg::JointState jointState; jointState.header.stamp = timeStamp; jointState.header.frame_id = ""; diff --git a/ocs2_ros_interfaces/package.xml b/ocs2_ros_interfaces/package.xml index 058bb790c..00914665b 100644 --- a/ocs2_ros_interfaces/package.xml +++ b/ocs2_ros_interfaces/package.xml @@ -15,8 +15,6 @@ eigen3_cmake_module eigen3_cmake_module - cmake_modules - eigen rclcpp ocs2_msgs From a53c81208c30faabd3b6b4f97d269f55e702c2f6 Mon Sep 17 00:00:00 2001 From: AnnikaWU <109574074+AnnikaWU@users.noreply.github.com> Date: Tue, 18 Nov 2025 09:49:43 +0800 Subject: [PATCH 42/42] Modify for dual-arm ACT+OCS2+MUJOCO in simulation (#57) * add single-arm aloha in mujoco simualtion * add dual-arm launch file and modify for dual-arm in mujoco simualtion * modify for general mpc and mrt node * modify for better compatibility * remove unnecessary modification and ros1 launch file for aloha * Update manipulator_aloha_dual_arm.launch.py --- log/COLCON_IGNORE | 0 log/latest | 1 + log/latest_build | 1 + .../config/aloha/task.info | 337 +++++++++++ .../config/aloha_dual_arm/task.info | 568 ++++++++++++++++++ .../MobileManipulatorInterface.h | 4 + .../src/MobileManipulatorInterface.cpp | 3 + .../launch/manipulator_aloha.launch.py | 69 +++ .../manipulator_aloha_dual_arm.launch.py | 135 +++++ .../src/MobileManipulatorDummyMRT.cpp | 9 +- .../src/MobileManipulatorMpcNode.cpp | 48 +- ocs2_ros_interfaces/CMakeLists.txt | 51 +- .../common/RosMsgConversions.h | 6 + .../mpc/MPC_ROS_Interface.h | 10 + .../synchronized_module/RosReferenceManager.h | 12 + ocs2_ros_interfaces/package.xml | 2 + .../src/common/RosMsgConversions.cpp | 17 + .../src/mpc/MPC_ROS_Interface.cpp | 41 ++ .../src/mrt/MRT_ROS_Dummy_Loop.cpp | 8 +- .../src/mrt/MRT_ROS_Interface.cpp | 2 +- .../RosReferenceManager.cpp | 64 ++ 21 files changed, 1366 insertions(+), 22 deletions(-) create mode 100644 log/COLCON_IGNORE create mode 120000 log/latest create mode 120000 log/latest_build create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator/config/aloha/task.info create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator/config/aloha_dual_arm/task.info create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_aloha.launch.py create mode 100644 ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_aloha_dual_arm.launch.py diff --git a/log/COLCON_IGNORE b/log/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/log/latest b/log/latest new file mode 120000 index 000000000..b57d247c7 --- /dev/null +++ b/log/latest @@ -0,0 +1 @@ +latest_build \ No newline at end of file diff --git a/log/latest_build b/log/latest_build new file mode 120000 index 000000000..0210ecd9a --- /dev/null +++ b/log/latest_build @@ -0,0 +1 @@ +build_2025-10-23_11-13-03 \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/config/aloha/task.info b/ocs2_robotic_examples/ocs2_mobile_manipulator/config/aloha/task.info new file mode 100644 index 000000000..2a68267c4 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/config/aloha/task.info @@ -0,0 +1,337 @@ +; robot model meta-information +model_information { + manipulatorModelType 0 // 0: Default-arm, 1: Wheel-based manipulator, 2: Floating-arm manipulator, 3: Fully actuated floating-arm manipulator + + ; motion joints in the URDF to consider fixed + removeJoints { + } + + ; base frame of the robot (from URDF) + baseFrame "vx300s/base_link" + ; end-effector frame of the robot (from URDF) + eeFrame "vx300s/gripper_link" +} + +model_settings +{ + usePreComputation true + recompileLibraries true +} + +; DDP settings +ddp +{ + algorithm SLQ + + nThreads 3 + threadPriority 50 + + maxNumIterations 1 + minRelCost 0.1 + constraintTolerance 1e-3 + + displayInfo false + displayShortSummary false + checkNumericalStability false + debugPrintRollout false + debugCaching false + + AbsTolODE 1e-5 + RelTolODE 1e-3 + maxNumStepsPerSecond 100000 + timeStep 1e-3 + backwardPassIntegratorType ODE45 + + constraintPenaltyInitialValue 20.0 + constraintPenaltyIncreaseRate 2.0 + + preComputeRiccatiTerms true + + useFeedbackPolicy false + + strategy LINE_SEARCH + lineSearch + { + minStepLength 1e-2 + maxStepLength 1.0 + hessianCorrectionStrategy DIAGONAL_SHIFT + hessianCorrectionMultiple 1e-3 + } +} + +; Rollout settings +rollout +{ + AbsTolODE 1e-5 + RelTolODE 1e-3 + timeStep 1e-2 + integratorType ODE45 + maxNumStepsPerSecond 100000 + checkNumericalStability false +} + +; MPC settings +mpc +{ + timeHorizon 1.0 ; [s] + solutionTimeWindow 0.2 ; [s] + coldStart false + + debugPrint false + + mpcDesiredFrequency 100 ; [Hz] + mrtDesiredFrequency 400 ; [Hz] +} + +; initial state +initialState +{ + ; initial state for the different types of arm base DOFs + base + { + defaultManipulator + { + } + + floatingArmManipulator + { + (0,0) 0.0 ; position x + (1,0) 0.0 ; position y + (2,0) 0.5 ; position z + (3,0) 0.0 ; euler angle z + (4,0) 0.0 ; euler angle y + (5,0) 1.7 ; euler angle x + } + + fullyActuatedFloatingArmManipulator + { + (0,0) 0.0 ; position x + (1,0) 0.0 ; position y + (2,0) 0.0 ; position z + (3,0) 0.0 ; euler angle z + (4,0) 0.0 ; euler angle y + (5,0) 0.0 ; euler angle x + } + + wheelBasedMobileManipulator + { + (0,0) 0.0 ; position x + (1,0) 0.0 ; position y + (2,0) 0.0 ; heading + } + } + + ; initial state for the arm DOFs // get from urdf? need to check arm num + arm + { + (0,0) 0.0 ; arm_1 + (1,0) -0.96 ; arm_2 + (2,0) 1.16 ; arm_3 + (3,0) 0.0 ; arm_4 + (4,0) -0.3 ; arm_5 + (5,0) 0.0 ; arm_6 + (6,0) 0.024 ; arm_7 + (7,0) 0.024 ; arm_8 + } +} + +inputCost +{ + ; control weight matrix + R + { + ; input costs for the different types of arm base DOFs + base + { + defaultManipulator + { + } + + floatingArmManipulator + { + } + + fullyActuatedFloatingArmManipulator + { + scaling 1e-2 + (0,0) 5.0 ; position x + (1,1) 5.0 ; position y + (2,2) 5.0 ; position z + (3,3) 5.0 ; euler angle z + (4,4) 5.0 ; euler angle y + (5,5) 5.0 ; euler angle x + } + + wheelBasedMobileManipulator + { + scaling 1e-2 + (0,0) 5.0 ; forward velocity + (1,1) 5.0 ; turning velocity + } + } + + ; input costs for the arm DOFs // need to check arm num + arm + { + scaling 1e-2 + + (0,0) 1.0 ; arm_1 velocity + (1,1) 1.0 ; arm_2 velocity + (2,2) 1.0 ; arm_3 velocity + (3,3) 1.0 ; arm_4 velocity + (4,4) 1.0 ; arm_5 velocity + (5,5) 1.0 ; arm_6 velocity + (6,6) 1.0 ; arm_7 velocity + (7,7) 1.0 ; arm_8 velocity + } + } +} + +endEffector // need to update data +{ + ; end effector quadratic penalty scaling + muPosition 50.0 + muOrientation 25.0 +} + +finalEndEffector // need to update data +{ + muPosition 50.0 + muOrientation 25.0 +} + +selfCollision +{ + ; activate self-collision constraint + activate false + + ; Self Collision raw object pairs // need to add + collisionObjectPairs + { + } + + ; Self Collision pairs // need to add + collisionLinkPairs + { + } + + ; minimum distance allowed between the pairs + minimumDistance 0.05 + + ; relaxed log barrier mu + mu 1e-2 + + ; relaxed log barrier delta + delta 1e-3 +} + +; Only applied for arm joints: limits parsed from URDF // need to add +jointPositionLimits +{ + ; activate constraint + activate false + + ; relaxed log barrier mu + mu 1e-3 + + ; relaxed log barrier delta + delta 1e-4 +} + +jointVelocityLimits +{ + ; relaxed log barrier mu + mu 0.01 + + ; relaxed log barrier delta + delta 1e-3 + + lowerBound + { + ; velocity limits for the different types of arm base DOFs + base + { + defaultManipulator + { + } + + floatingArmManipulator + { + } + + fullyActuatedFloatingArmManipulator + { + (0,0) -0.1 ; linear velocity x + (1,0) -0.1 ; linear velocity y + (2,0) -0.1 ; linear velocity z + (3,0) -0.3 ; euler angle velocity z + (4,0) -0.3 ; euler angle velocity y + (5,0) -0.3 ; euler angle velocity x + } + + wheelBasedMobileManipulator + { + (0,0) -0.1 ; forward velocity + (1,0) -0.3 ; turning velocity + } + } + + ; velocity limits for the arm DOFs // all need to change from urdf + arm + { + (0,0) -3.141592653589793 ; arm_1 [rad/s] + (1,0) -3.141592653589793 ; arm_2 [rad/s] + (2,0) -3.141592653589793 ; arm_3 [rad/s] + (3,0) -3.141592653589793 ; arm_4 [rad/s] + (4,0) -3.141592653589793 ; arm_5 [rad/s] + (5,0) -3.141592653589793 ; arm_6 [rad/s] + (6,0) -3.141592653589793 ; arm_7 [rad/s] + (7,0) -3.141592653589793 ; arm_8 [rad/s] + } + } + + + upperBound + { + ; velocity limits for the different types of arm base DOFs + base + { + defaultManipulator + { + } + + floatingArmManipulator + { + } + + fullyActuatedFloatingArmManipulator + { + (0,0) 0.1 ; linear velocity x + (1,0) 0.1 ; linear velocity y + (2,0) 0.1 ; linear velocity z + (3,0) 0.3 ; euler angle velocity z + (4,0) 0.3 ; euler angle velocity y + (5,0) 0.3 ; euler angle velocity x + } + + wheelBasedMobileManipulator + { + (0,0) 0.1 ; forward velocity + (1,0) 0.3 ; turning velocity + } + } + + ; velocity limits for the arm DOFs // all need to change from urdf + arm + { + (0,0) 3.141592653589793 ; arm_1 [rad/s] + (1,0) 3.141592653589793 ; arm_2 [rad/s] + (2,0) 3.141592653589793 ; arm_3 [rad/s] + (3,0) 3.141592653589793 ; arm_4 [rad/s] + (4,0) 3.141592653589793 ; arm_5 [rad/s] + (5,0) 3.141592653589793 ; arm_6 [rad/s] + (6,0) 3.141592653589793 ; arm_7 [rad/s] + (7,0) 3.141592653589793 ; arm_8 [rad/s] + } + } +} diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/config/aloha_dual_arm/task.info b/ocs2_robotic_examples/ocs2_mobile_manipulator/config/aloha_dual_arm/task.info new file mode 100644 index 000000000..c270c13d8 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/config/aloha_dual_arm/task.info @@ -0,0 +1,568 @@ +; robot model meta-information +model_information { + manipulatorModelType 4 // 0: Default-arm, 1: Wheel-based manipulator, 2: Floating-arm manipulator, 3: Fully actuated floating-arm manipulator, 4: Dual-arm with fixed base + + ; motion joints in the URDF to consider fixed + removeJoints { + left + { + [0] "right_base_joint" + [1] "vx300s_right/waist" + [2] "vx300s_right/shoulder" + [3] "vx300s_right/elbow" + [4] "vx300s_right/forearm_roll" + [5] "vx300s_right/wrist_angle" + [6] "vx300s_right/wrist_rotate" + } + right + { + [0] "left_base_joint" + [1] "vx300s_left/waist" + [2] "vx300s_left/shoulder" + [3] "vx300s_left/elbow" + [4] "vx300s_left/forearm_roll" + [5] "vx300s_left/wrist_angle" + [6] "vx300s_left/wrist_rotate" + } + } + + ; base frame of the robot (from URDF) + leftBaseFrame "vx300s_left/base_link" + rightBaseFrame "vx300s_right/base_link" + + ; end-effector frames of the robot (from URDF) + leftEeFrame "vx300s_left/gripper_link" + rightEeFrame "vx300s_right/gripper_link" + +} + +model_settings +{ + usePreComputation true + recompileLibraries true +} + +; DDP settings +ddp +{ + algorithm SLQ + + nThreads 3 + threadPriority 50 + + maxNumIterations 1 + minRelCost 0.1 + constraintTolerance 1e-3 + + displayInfo false + displayShortSummary false + checkNumericalStability false + debugPrintRollout false + debugCaching false + + AbsTolODE 1e-5 + RelTolODE 1e-3 + maxNumStepsPerSecond 100000 + timeStep 1e-3 + backwardPassIntegratorType ODE45 + + constraintPenaltyInitialValue 20.0 + constraintPenaltyIncreaseRate 2.0 + + preComputeRiccatiTerms true + + useFeedbackPolicy false + + strategy LINE_SEARCH + lineSearch + { + minStepLength 1e-2 + maxStepLength 1.0 + hessianCorrectionStrategy DIAGONAL_SHIFT + hessianCorrectionMultiple 1e-3 + } +} + +; Rollout settings +rollout +{ + AbsTolODE 1e-5 + RelTolODE 1e-3 + timeStep 1e-1 // change from 1e-2 + integratorType ODE45 + maxNumStepsPerSecond 10000 // change from 100000 + checkNumericalStability false +} + +; MPC settings +mpc +{ + ; timeHorizon 1.0 ; [s] + timeHorizon 2.0 ; [s] extend timeHorizon for unstable ALOHA feedback state freq (next feedback state time must <= current feedback time+timeHorizon) + solutionTimeWindow 0.2 ; [s] + coldStart false + + debugPrint false + + mpcDesiredFrequency 50 ; [Hz] change from 100Hz as to apply realtime strategy in ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp + mrtDesiredFrequency 200 ; [Hz] change from 400Hz to allocate with ALOHA +} + +; initial state +initialState +{ + ; initial state for the different types of arm base DOFs + base + { + defaultManipulator + { + } + + floatingArmManipulator + { + (0,0) 0.0 ; position x + (1,0) 0.0 ; position y + (2,0) 0.5 ; position z + (3,0) 0.0 ; euler angle z + (4,0) 0.0 ; euler angle y + (5,0) 1.7 ; euler angle x + } + + fullyActuatedFloatingArmManipulator + { + (0,0) 0.0 ; position x + (1,0) 0.0 ; position y + (2,0) 0.0 ; position z + (3,0) 0.0 ; euler angle z + (4,0) 0.0 ; euler angle y + (5,0) 0.0 ; euler angle x + } + + wheelBasedMobileManipulator + { + (0,0) 0.0 ; position x + (1,0) 0.0 ; position y + (2,0) 0.0 ; heading + } + } + + ; initial state for the dual arm DOFs // need to check arm num + dualArm + { + leftArm + { + (0,0) 0.0 ; arm_1 + (1,0) -0.96 ; arm_2 + (2,0) 1.16 ; arm_3 + (3,0) 0.0 ; arm_4 + (4,0) -0.3 ; arm_5 + (5,0) 0.0 ; arm_6 + (6,0) 0.024 ; arm_7 + (7,0) 0.024 ; arm_8 + } + + rightArm + { + (0,0) 0.0 ; arm_1 + (1,0) -0.96 ; arm_2 + (2,0) 1.16 ; arm_3 + (3,0) 0.0 ; arm_4 + (4,0) -0.3 ; arm_5 + (5,0) 0.0 ; arm_6 + (6,0) 0.024 ; arm_7 + (7,0) 0.024 ; arm_8 + } + + } +} + +inputCost +{ + ; control weight matrix + R + { + ; input costs for the different types of arm base DOFs + base + { + defaultManipulator + { + } + + floatingArmManipulator + { + } + + fullyActuatedFloatingArmManipulator + { + scaling 1e-2 + (0,0) 5.0 ; position x + (1,1) 5.0 ; position y + (2,2) 5.0 ; position z + (3,3) 5.0 ; euler angle z + (4,4) 5.0 ; euler angle y + (5,5) 5.0 ; euler angle x + } + + wheelBasedMobileManipulator + { + scaling 1e-2 + (0,0) 5.0 ; forward velocity + (1,1) 5.0 ; turning velocity + } + } + + ; input costs for the arm DOFs // need to check arm num + dualArm + { + leftArm + { + scaling 1e-2; + + (0,0) 1.0 ; arm_1 velocity + (1,1) 1.0 ; arm_2 velocity + (2,2) 1.0 ; arm_3 velocity + (3,3) 1.0 ; arm_4 velocity + (4,4) 1.0 ; arm_5 velocity + (5,5) 1.0 ; arm_6 velocity + (6,6) 1.0 ; arm_7 velocity + (7,7) 1.0 ; arm_8 velocity + } + + rightArm + { + scaling 1e-2; + + (0,0) 1.0 ; arm_1 velocity + (1,1) 1.0 ; arm_2 velocity + (2,2) 1.0 ; arm_3 velocity + (3,3) 1.0 ; arm_4 velocity + (4,4) 1.0 ; arm_5 velocity + (5,5) 1.0 ; arm_6 velocity + (6,6) 1.0 ; arm_7 velocity + (7,7) 1.0 ; arm_8 velocity + } + } + } +} + +endEffector +{ + leftArm + { + ; end effector quadratic penalty scaling + muPosition 50.0 + muOrientation 25.0 + } + + rightArm + { + ; end effector quadratic penalty scaling + muPosition 50.0 + muOrientation 25.0 + } + +} + +finalEndEffector +{ + leftArm + { + muPosition 50.0 + muOrientation 25.0 + } + + rightArm + { + muPosition 50.0 + muOrientation 25.0 + } + +} + +selfCollision +{ + ; activate self-collision constraint + activate false + + ; Self Collision raw object pairs // need to add + collisionObjectPairs + { + } + + ; Self Collision pairs // need to add + collisionLinkPairs + { + } + + ; minimum distance allowed between the pairs + minimumDistance 0.05 + + ; relaxed log barrier mu + mu 1e-2 + + ; relaxed log barrier delta + delta 1e-3 +} + +; Only applied for arm joints: limits parsed from URDF // need to add +jointPositionLimits +{ + ; activate constraint + activate true + + ; relaxed log barrier mu + mu 1e-3 + + ; relaxed log barrier delta + delta 1e-4 + + lowerBound + { + ; position limits for the different types of arm base DOFs + base + { + defaultManipulator + { + } + + floatingArmManipulator + { + } + + ; add for avoiding bug + fullyActuatedFloatingArmManipulator + { + (0,0) -0.1 ; linear velocity x + (1,0) -0.1 ; linear velocity y + (2,0) -0.1 ; linear velocity z + (3,0) -0.3 ; euler angle velocity z + (4,0) -0.3 ; euler angle velocity y + (5,0) -0.3 ; euler angle velocity x + } + + ; add for avoiding bug + wheelBasedMobileManipulator + { + (0,0) -0.1 ; forward velocity + (1,0) -0.3 ; turning velocity + } + } + + ; position limits for the arm DOFs // all need to change from urdf + dualArm + { + leftArm + { + (0,0) -3.141582653589793 ; arm_1 + (1,0) -1.8500490071139892 ; arm_2 + (2,0) -1.7627825445142729 ; arm_3 + (3,0) -3.141592653589793 ; arm_4 + (4,0) -1.8675022996339325 ; arm_5 + (5,0) -3.141592653589793 ; arm_6 + (6,0) 0.021 ; left_finger + (7,0) -0.057 ; right_finger + } + + rightArm + { + (0,0) -3.141582653589793 ; arm_1 + (1,0) -1.8500490071139892 ; arm_2 + (2,0) -1.7627825445142729 ; arm_3 + (3,0) -3.141592653589793 ; arm_4 + (4,0) -1.8675022996339325 ; arm_5 + (5,0) -3.141592653589793 ; arm_6 + (6,0) 0.021 ; left_finger + (7,0) -0.057 ; right_finger + } + } + } + + upperBound + { + ;position limits for the different types of arm base DOFs + base + { + defaultManipulator + { + } + + floatingArmManipulator + { + } + + ; add for avoiding bug + fullyActuatedFloatingArmManipulator + { + (0,0) 0.1 ; linear velocity x + (1,0) 0.1 ; linear velocity y + (2,0) 0.1 ; linear velocity z + (3,0) 0.3 ; euler angle velocity z + (4,0) 0.3 ; euler angle velocity y + (5,0) 0.3 ; euler angle velocity x + } + + ; add for avoiding bug + wheelBasedMobileManipulator + { + (0,0) 0.1 ; forward velocity + (1,0) 0.3 ; turning velocity + } + } + + ; position limits for the arm DOFs // all need to change from urdf + dualArm + { + leftArm + { + (0,0) 3.141582653589793 ; arm_1 + (1,0) 1.2566370614359172 ; arm_2 + (2,0) 1.6057029118347832 ; arm_3 + (3,0) 3.141592653589793 ; arm_4 + (4,0) 2.234021442552742 ; arm_5 + (5,0) 3.141592653589793 ; arm_6 + (6,0) 0.057 ; left_finger + (7,0) -0.021 ; right_finger + } + + rightArm + { + (0,0) 3.141582653589793 ; arm_1 + (1,0) 1.2566370614359172 ; arm_2 + (2,0) 1.6057029118347832 ; arm_3 + (3,0) 3.141592653589793 ; arm_4 + (4,0) 2.234021442552742 ; arm_5 + (5,0) 3.141592653589793 ; arm_6 + (6,0) 0.057 ; left_finger + (7,0) -0.021 ; right_finger + } + } + } +} + + +jointVelocityLimits +{ + ; relaxed log barrier mu + mu 0.01 + + ; relaxed log barrier delta + delta 1e-3 + + lowerBound + { + ; velocity limits for the different types of arm base DOFs + base + { + defaultManipulator + { + } + + floatingArmManipulator + { + } + + fullyActuatedFloatingArmManipulator + { + (0,0) -0.1 ; linear velocity x + (1,0) -0.1 ; linear velocity y + (2,0) -0.1 ; linear velocity z + (3,0) -0.3 ; euler angle velocity z + (4,0) -0.3 ; euler angle velocity y + (5,0) -0.3 ; euler angle velocity x + } + + wheelBasedMobileManipulator + { + (0,0) -0.1 ; forward velocity + (1,0) -0.3 ; turning velocity + } + } + + ; velocity limits for the arm DOFs // all need to change from urdf + dualArm + { + leftArm + { + (0,0) -3.141592653589793 ; arm_1 [rad/s] + (1,0) -3.141592653589793 ; arm_2 [rad/s] + (2,0) -3.141592653589793 ; arm_3 [rad/s] + (3,0) -3.141592653589793 ; arm_4 [rad/s] + (4,0) -3.141592653589793 ; arm_5 [rad/s] + (5,0) -3.141592653589793 ; arm_6 [rad/s] + (6,0) -1.000000000000000 ; left_finger [rad/s] + (7,0) -1.000000000000000 ; right_finger [rad/s] + } + + rightArm + { + (0,0) -3.141592653589793 ; arm_1 [rad/s] + (1,0) -3.141592653589793 ; arm_2 [rad/s] + (2,0) -3.141592653589793 ; arm_3 [rad/s] + (3,0) -3.141592653589793 ; arm_4 [rad/s] + (4,0) -3.141592653589793 ; arm_5 [rad/s] + (5,0) -3.141592653589793 ; arm_6 [rad/s] + (6,0) -1.000000000000000 ; left_finger [rad/s] + (7,0) -1.000000000000000 ; right_finger [rad/s] + } + } + } + + upperBound + { + ; velocity limits for the different types of arm base DOFs + base + { + defaultManipulator + { + } + + floatingArmManipulator + { + } + + fullyActuatedFloatingArmManipulator + { + (0,0) 0.1 ; linear velocity x + (1,0) 0.1 ; linear velocity y + (2,0) 0.1 ; linear velocity z + (3,0) 0.3 ; euler angle velocity z + (4,0) 0.3 ; euler angle velocity y + (5,0) 0.3 ; euler angle velocity x + } + + wheelBasedMobileManipulator + { + (0,0) 0.1 ; forward velocity + (1,0) 0.3 ; turning velocity + } + } + + ; velocity limits for the arm DOFs // all need to change from urdf + dualArm + { + leftArm + { + (0,0) 3.141592653589793 ; arm_1 [rad/s] + (1,0) 3.141592653589793 ; arm_2 [rad/s] + (2,0) 3.141592653589793 ; arm_3 [rad/s] + (3,0) 3.141592653589793 ; arm_4 [rad/s] + (4,0) 3.141592653589793 ; arm_5 [rad/s] + (5,0) 3.141592653589793 ; arm_6 [rad/s] + (6,0) 1.000000000000000 ; left_finger [rad/s] + (7,0) 1.000000000000000 ; right_finger [rad/s] + } + + rightArm + { + (0,0) 3.141592653589793 ; arm_1 [rad/s] + (1,0) 3.141592653589793 ; arm_2 [rad/s] + (2,0) 3.141592653589793 ; arm_3 [rad/s] + (3,0) 3.141592653589793 ; arm_4 [rad/s] + (4,0) 3.141592653589793 ; arm_5 [rad/s] + (5,0) 3.141592653589793 ; arm_6 [rad/s] + (6,0) 1.000000000000000 ; left_finger [rad/s] + (7,0) 1.000000000000000 ; right_finger [rad/s] + } + } + } +} \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/include/ocs2_mobile_manipulator/MobileManipulatorInterface.h b/ocs2_robotic_examples/ocs2_mobile_manipulator/include/ocs2_mobile_manipulator/MobileManipulatorInterface.h index dacd6b9d3..d4cca55a3 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/include/ocs2_mobile_manipulator/MobileManipulatorInterface.h +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/include/ocs2_mobile_manipulator/MobileManipulatorInterface.h @@ -63,6 +63,8 @@ class MobileManipulatorInterface final : public RobotInterface { const vector_t& getInitialState() { return initialState_; } + const int getEeFrameId() { return eeFrameId_; } + ddp::Settings& ddpSettings() { return ddpSettings_; } mpc::Settings& mpcSettings() { return mpcSettings_; } @@ -102,6 +104,8 @@ class MobileManipulatorInterface final : public RobotInterface { ManipulatorModelInfo manipulatorModelInfo_; vector_t initialState_; + + int eeFrameId_; }; } // namespace mobile_manipulator diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/src/MobileManipulatorInterface.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator/src/MobileManipulatorInterface.cpp index 1fdee8f26..01cbb918d 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/src/MobileManipulatorInterface.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/src/MobileManipulatorInterface.cpp @@ -119,6 +119,9 @@ MobileManipulatorInterface::MobileManipulatorInterface(const std::string& taskFi pinocchioInterfacePtr_.reset(new PinocchioInterface(createPinocchioInterface(urdfFile, modelType, removeJointNames))); std::cerr << *pinocchioInterfacePtr_; + // get end Effector ID + eeFrameId_ = pinocchioInterfacePtr_->getModel().getFrameId(eeFrame); + // ManipulatorModelInfo manipulatorModelInfo_ = mobile_manipulator::createManipulatorModelInfo(*pinocchioInterfacePtr_, modelType, baseFrame, eeFrame); diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_aloha.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_aloha.launch.py new file mode 100644 index 000000000..f375a93eb --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_aloha.launch.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python3 +"""Launch file for ALOHA mobile manipulator example.""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + # Package directories + ocs2_mobile_manipulator_ros_dir = get_package_share_directory('ocs2_mobile_manipulator_ros') + interbotix_xsarm_descriptions_dir = get_package_share_directory('interbotix_xsarm_descriptions') + ocs2_mobile_manipulator_dir = get_package_share_directory('ocs2_mobile_manipulator') + + # Launch arguments + rviz_arg = DeclareLaunchArgument( + 'rviz', + default_value='true', + description='Enable rviz visualization' + ) + + debug_arg = DeclareLaunchArgument( + 'debug', + default_value='false', + description='Set nodes on debug mode' + ) + + urdf_file_arg = DeclareLaunchArgument( + 'urdfFile', + default_value=os.path.join(interbotix_xsarm_descriptions_dir, 'urdf', 'vx300s.urdf'), + description='The URDF model of the robot' + ) + + task_file_arg = DeclareLaunchArgument( + 'taskFile', + default_value=os.path.join(ocs2_mobile_manipulator_dir, 'config', 'aloha', 'task.info'), + description='The task file for the mpc' + ) + + lib_folder_arg = DeclareLaunchArgument( + 'libFolder', + default_value=os.path.join(ocs2_mobile_manipulator_dir, 'auto_generated', 'aloha'), + description='The library folder to generate CppAD codegen into' + ) + + # Include main launch file + mobile_manipulator_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ocs2_mobile_manipulator_ros_dir, 'launch', 'mobile_manipulator.launch.py') + ), + launch_arguments={ + 'rviz': LaunchConfiguration('rviz'), + 'debug': LaunchConfiguration('debug'), + 'urdfFile': LaunchConfiguration('urdfFile'), + 'taskFile': LaunchConfiguration('taskFile'), + 'libFolder': LaunchConfiguration('libFolder'), + }.items() + ) + + return LaunchDescription([ + rviz_arg, + debug_arg, + urdf_file_arg, + task_file_arg, + lib_folder_arg, + mobile_manipulator_launch + ]) \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_aloha_dual_arm.launch.py b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_aloha_dual_arm.launch.py new file mode 100644 index 000000000..a389bf242 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/launch/manipulator_aloha_dual_arm.launch.py @@ -0,0 +1,135 @@ +#!/usr/bin/env python3 +"""Launch file for ALOHA dual arm mobile manipulator example.""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.conditions import IfCondition + +def generate_launch_description(): + # Package directories + ocs2_mobile_manipulator_ros_dir = get_package_share_directory('ocs2_mobile_manipulator_ros') + ocs2_robotic_assets_dir = get_package_share_directory('ocs2_robotic_assets') + ocs2_mobile_manipulator_dir = get_package_share_directory('ocs2_mobile_manipulator') + + # Launch arguments + rviz_arg = DeclareLaunchArgument( + 'rviz', + default_value='false', + description='Enable rviz visualization' + ) + + debug_arg = DeclareLaunchArgument( + 'debug', + default_value='false', + description='Set nodes on debug mode' + ) + + urdf_file_arg = DeclareLaunchArgument( + 'urdfFile', + default_value=os.path.join(ocs2_robotic_assets_dir, 'resources', 'mobile_manipulator', 'aloha', 'urdf', 'vx300s.urdf'), + description='The URDF model of the robot' + ) + + task_file_arg = DeclareLaunchArgument( + 'taskFile', + default_value=os.path.join(ocs2_mobile_manipulator_dir, 'config', 'aloha', 'task.info'), + description='The task file for the mpc' + ) + + lib_folder_arg = DeclareLaunchArgument( + 'libFolder', + default_value=os.path.join(ocs2_mobile_manipulator_dir, 'auto_generated', 'aloha_dual_arm'), + description='The library folder to generate CppAD codegen into' + ) + + nodes = [] + + # left arm MPC node + nodes.append(Node( + package='ocs2_mobile_manipulator_ros', + # executable='mobile_manipulator_dual_arm_mpc_node', + executable='mobile_manipulator_mpc_node', + name='left_arm_mpc_node', + namespace='left_arm', + parameters=[ + {'armSide': 'LEFT'}, + {'robotName': 'mobile_manipulator'}, + {'taskFile': LaunchConfiguration('taskFile')}, + {'libFolder': LaunchConfiguration('libFolder')}, + {'urdfFile': LaunchConfiguration('urdfFile')} + ], + output='screen' + )) + + # right arm MPC node + nodes.append(Node( + package='ocs2_mobile_manipulator_ros', + # executable='mobile_manipulator_dual_arm_mpc_node', + executable='mobile_manipulator_mpc_node', + name='right_arm_mpc_node', + namespace='right_arm', + parameters=[ + {'armSide': 'RIGHT'}, + {'robotName': 'mobile_manipulator'}, + {'taskFile': LaunchConfiguration('taskFile')}, + {'libFolder': LaunchConfiguration('libFolder')}, + {'urdfFile': LaunchConfiguration('urdfFile')} + ], + output='screen' + )) + + # left arm MRT node + nodes.append(Node( + package='ocs2_mobile_manipulator_ros', + # executable='mobile_manipulator_dual_arm_dummy_mrt_node', + executable='mobile_manipulator_dummy_mrt_node', + name='left_arm_mrt_node', + namespace='left_arm', + parameters=[ + {'armSide': 'LEFT'}, + {'robotName': 'mobile_manipulator'}, + {'taskFile': LaunchConfiguration('taskFile')}, + {'libFolder': LaunchConfiguration('libFolder')}, + {'urdfFile': LaunchConfiguration('urdfFile')} + ], + output='screen' + )) + + # right arm MRT node + nodes.append(Node( + package='ocs2_mobile_manipulator_ros', + # executable='mobile_manipulator_dual_arm_dummy_mrt_node', + executable='mobile_manipulator_dummy_mrt_node', + name='right_arm_mrt_node', + namespace='right_arm', + parameters=[ + {'armSide': 'RIGHT'}, + {'robotName': 'mobile_manipulator'}, + {'taskFile': LaunchConfiguration('taskFile')}, + {'libFolder': LaunchConfiguration('libFolder')}, + {'urdfFile': LaunchConfiguration('urdfFile')} + ], + output='screen' + )) + + # # RViz node + # nodes.append(Node( + # package='rviz2', + # executable='rviz2', + # name='rviz2', + # arguments=['-d', os.path.join(ocs2_mobile_manipulator_ros_dir, 'rviz', 'mobile_manipulator.rviz')], + # condition=IfCondition(LaunchConfiguration('rviz')), + # output='screen' + # )) + + return LaunchDescription([ + rviz_arg, + debug_arg, + urdf_file_arg, + task_file_arg, + lib_folder_arg, + ] + nodes) diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp index 33d3a7027..f1f45809d 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp @@ -83,8 +83,13 @@ int main(int argc, char** argv) { // initial command vector_t initTarget(7); - initTarget.head(3) << 1, 0, 1; - initTarget.tail(4) << Eigen::Quaternion(1, 0, 0, 0).coeffs(); + if (taskFile.find("aloha") != std::string::npos) { + initTarget << 0.151811, 0, 0.295251, 0, -0.0499792, 0, 0.99875; // ALOHA waiting state (more suitable than sleeping state) + // initTarget << 0.0526199, 0.00505283, 0.159145, 0.0552305, 0.246959, 0.0233511, 0.967169; // ALOHA sleeping state + } else { + initTarget.head(3) << 1, 0, 1; + initTarget.tail(4) << Eigen::Quaternion(1, 0, 0, 0).coeffs(); + } const vector_t zeroInput = vector_t::Zero(interface.getManipulatorModelInfo().inputDim); const TargetTrajectories initTargetTrajectories({initObservation.time}, {initTarget}, {zeroInput}); diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp index 7dfc58d76..3513b4789 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp @@ -46,33 +46,57 @@ int main(int argc, char** argv) { rclcpp::NodeOptions() .allow_undeclared_parameters(true) .automatically_declare_parameters_from_overrides(true)); + + rclcpp::sleep_for(std::chrono::milliseconds(100)); // Get node parameters - std::string taskFile, libFolder, urdfFile; - // node->declare_parameter("taskFile", std::string("")); - // node->declare_parameter("libFolder", std::string("")); - // node->declare_parameter("urdfFile", std::string("")); + std::string taskFile, libFolder, urdfFile, armSide; taskFile = node->get_parameter("taskFile").as_string(); libFolder = node->get_parameter("libFolder").as_string(); urdfFile = node->get_parameter("urdfFile").as_string(); + // Check if armSide parameter exists, if not set default to "LEFT" + if (node->has_parameter("armSide")) { + armSide = node->get_parameter("armSide").as_string(); + std::cerr << "Set arm side: " << armSide << std::endl; + } else { + armSide = "LEFT"; + std::cerr << "armSide parameter not found in launch file, setting default to: " << armSide << std::endl; + } + std::cerr << "Loading task file: " << taskFile << std::endl; std::cerr << "Loading library folder: " << libFolder << std::endl; std::cerr << "Loading urdf file: " << urdfFile << std::endl; + // Robot interface MobileManipulatorInterface interface(taskFile, libFolder, urdfFile); // ROS ReferenceManager auto rosReferenceManagerPtr = std::make_shared(robotName, interface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(node); - // MPC - ocs2::GaussNewtonDDP_MPC mpc(interface.mpcSettings(), interface.ddpSettings(), interface.getRollout(), + // Check if taskFile contains "aloha" to decide which subscribe and launch method to use + if (taskFile.find("aloha") != std::string::npos) { + std::cerr << "For aloha robotic arm, using subscribe_act and launchNodes_mujoco method" << std::endl; + // Subscribe target from act + rosReferenceManagerPtr->subscribe_act(node, interface.getPinocchioInterface(), interface.getEeFrameId(), armSide); + // MPC + ocs2::GaussNewtonDDP_MPC mpc(interface.mpcSettings(), interface.ddpSettings(), interface.getRollout(), + interface.getOptimalControlProblem(), interface.getInitializer()); + mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); + // Launch MPC mujoco ROS node + MPC_ROS_Interface mpcNode(mpc, robotName); + mpcNode.launchNodes_mujoco(node); + } else { + std::cerr << "For non-aloha, using standard subscribe and launchNodes method" << std::endl; + // Subscribe target in Rviz + rosReferenceManagerPtr->subscribe(node); + // MPC + ocs2::GaussNewtonDDP_MPC mpc(interface.mpcSettings(), interface.ddpSettings(), interface.getRollout(), interface.getOptimalControlProblem(), interface.getInitializer()); - mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); - - // Launch MPC ROS node - MPC_ROS_Interface mpcNode(mpc, robotName); - mpcNode.launchNodes(node); + mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); + // Launch MPC ROS node + MPC_ROS_Interface mpcNode(mpc, robotName); + mpcNode.launchNodes(node); + } rclcpp::shutdown(); diff --git a/ocs2_ros_interfaces/CMakeLists.txt b/ocs2_ros_interfaces/CMakeLists.txt index 60b49ecd0..695d16b76 100644 --- a/ocs2_ros_interfaces/CMakeLists.txt +++ b/ocs2_ros_interfaces/CMakeLists.txt @@ -21,6 +21,16 @@ find_package(interactive_markers REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3) +find_package(ocs2_pinocchio_interface REQUIRED) +pkg_check_modules(pinocchio REQUIRED pinocchio) +find_package(pinocchio REQUIRED) + +find_package(urdf REQUIRED) +find_package(urdfdom REQUIRED) + +pkg_check_modules(hpp-fcl REQUIRED hpp-fcl) +find_package(hpp-fcl REQUIRED) + ########### ## Build ## ########### @@ -36,6 +46,16 @@ set(dependencies interactive_markers Boost Eigen3 + ocs2_pinocchio_interface + pinocchio + hpp-fcl + urdf + urdfdom + Eigen3 +) + +link_directories( + ${pinocchio_LIBRARY_DIRS} ) add_library(${PROJECT_NAME} SHARED @@ -61,9 +81,26 @@ target_include_directories(${PROJECT_NAME} PUBLIC ${includes_${PROJECT_NAME}} ) ament_target_dependencies(${PROJECT_NAME} - ${dependencies} + rclcpp + ocs2_msgs + ocs2_core + ocs2_mpc + std_msgs + visualization_msgs + geometry_msgs + interactive_markers + ocs2_pinocchio_interface + pinocchio + hpp-fcl + urdf + urdfdom + Eigen3 +) + +target_link_libraries(${PROJECT_NAME} + ${Boost_LIBRARIES} + ${pinocchio_LIBRARIES} ) -target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) add_executable(test_custom_callback_queue test/test_custom_callback_queue.cpp @@ -105,11 +142,11 @@ find_package(cmake_clang_tools QUIET) if(cmake_clang_tools_FOUND) message(STATUS "Run clang tooling for target " ${PROJECT_NAME}_lintTarget) add_clang_tooling( - TARGETS ${PROJECT_NAME}_lintTarget + TARGETS ${PROJECT_NAME}_lintTarget SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include CF_WERROR -) + ) endif(cmake_clang_tools_FOUND) ############# @@ -130,7 +167,9 @@ install(TARGETS multiplot_remap DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY multiplot DESTINATION include/${PROJECT_NAME}/ ) -ament_export_dependencies(${dependencies}) +ament_export_dependencies( + ${dependencies} +) ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ############# @@ -143,4 +182,4 @@ ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ## to see the summary of unit test results run ## $ catkin_test_results ../../../build/ocs2_ros_interfaces -ament_package() +ament_package() \ No newline at end of file diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h index 1c59c8835..aa409ca75 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h @@ -46,6 +46,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +// ACT messages +#include + namespace ocs2 { namespace ros_msg_conversions { @@ -73,6 +76,9 @@ ocs2_msgs::msg::MpcTargetTrajectories createTargetTrajectoriesMsg( TargetTrajectories readTargetTrajectoriesMsg( const ocs2_msgs::msg::MpcTargetTrajectories& targetTrajectoriesMsg); +/** Reads the ACT target qpos message. */ +vector_t readActTargetQposMsg(const std_msgs::msg::Float32MultiArray& actTargetMsg); + /** * Creates the performance indices message. * diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h index 2de03e08e..473db49a6 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h @@ -53,6 +53,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +// ACT messages +#include + #define PUBLISH_THREAD namespace ocs2 { @@ -101,6 +104,13 @@ class MPC_ROS_Interface { */ void launchNodes(const rclcpp::Node::SharedPtr& node); + /** +* This is the main routine which launches all the nodes required for MPC to run which includes: +* (1) The MPC policy publisher (either feedback or feedforward policy). +* (2) The observation subscriber which gets the current measured state to invoke the MPC run routine. +*/ + void launchNodes_mujoco(const rclcpp::Node::SharedPtr& node); + protected: /** * Callback to reset MPC. diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h index 0dbfbc625..6a5ebee8e 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h @@ -39,6 +39,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include +#include + namespace ocs2 { /** @@ -84,6 +87,14 @@ class RosReferenceManager final : public ReferenceManagerDecorator { */ void subscribe(const rclcpp::Node::SharedPtr& node); + /** + * Subscribers to "topicPrefix_mode_schedule" and "act_aloha_target_qpos" + * topics to receive respectively: + * (1) ModeSchedule : The predefined mode schedule for time-triggered hybrid systems. + * (2) TargetTrajectories : The commanded TargetTrajectories from act. + */ + void subscribe_act(const rclcpp::Node::SharedPtr& node, PinocchioInterface pinocchioInterface, int eeFrameId, const std::string& armSide); + private: const std::string topicPrefix_; rclcpp::Node::SharedPtr node_; @@ -91,6 +102,7 @@ class RosReferenceManager final : public ReferenceManagerDecorator { modeScheduleSubscriber_; rclcpp::Subscription::SharedPtr targetTrajectoriesSubscriber_; + rclcpp::Subscription::SharedPtr actTargetTrajectoriesSubscriber_; }; /******************************************************************************************************/ diff --git a/ocs2_ros_interfaces/package.xml b/ocs2_ros_interfaces/package.xml index 00914665b..6f38bc975 100644 --- a/ocs2_ros_interfaces/package.xml +++ b/ocs2_ros_interfaces/package.xml @@ -20,6 +20,8 @@ ocs2_msgs ocs2_core ocs2_mpc + ocs2_pinocchio_interface + hpp-fcl std_msgs visualization_msgs geometry_msgs diff --git a/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp b/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp index 9ade81948..bda354292 100644 --- a/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp +++ b/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp @@ -242,6 +242,23 @@ TargetTrajectories readTargetTrajectoriesMsg( desiredInputTrajectory}; } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +vector_t readActTargetQposMsg(const std_msgs::msg::Float32MultiArray& actTargetMsg) { + + if (actTargetMsg.data.empty()) { + throw std::runtime_error("Input array is empty."); + } + + std::vector act_state(actTargetMsg.data.size()); + for (int i = 0; i < actTargetMsg.data.size(); i++){ + act_state[i] = actTargetMsg.data.at(i); + } + + return Eigen::Map(act_state.data(), act_state.size()).cast(); +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ diff --git a/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp b/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp index 24b8ba3d2..10c8f039c 100644 --- a/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp +++ b/ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp @@ -389,4 +389,45 @@ void MPC_ROS_Interface::launchNodes(const rclcpp::Node::SharedPtr& node) { spin(); } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MPC_ROS_Interface::launchNodes_mujoco(const rclcpp::Node::SharedPtr& node) { + RCLCPP_INFO(LOGGER, "MPC node is setting up ..."); + node_ = node; + + // Observation subscriber + // modify to /left_arm/pose and /right_arm/pose when applying dual-arm + mpcObservationSubscriber_ = node_->create_subscription( + "pose", + 1, + std::bind(&MPC_ROS_Interface::mpcObservationCallback, this, std::placeholders::_1) + ); + + // MPC publisher + mpcPolicyPublisher_ = node_->create_publisher( + topicPrefix_ + "_mpc_policy", + 1 + ); + + // MPC reset service server + mpcResetServiceServer_ = node_->create_service( + topicPrefix_ + "_mpc_reset", + [this](const std::shared_ptr& request, + const std::shared_ptr& response) { + return resetMpcCallback(request, response); + }); + + // display +#ifdef PUBLISH_THREAD + RCLCPP_INFO(LOGGER, "Publishing SLQ-MPC messages on a separate thread."); +#endif + + RCLCPP_INFO(LOGGER, "MPC node is ready."); + + // spin + spin(); +} + + } // namespace ocs2 diff --git a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp index a9c760c10..02610fe14 100644 --- a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp +++ b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Dummy_Loop.cpp @@ -101,7 +101,8 @@ void MRT_ROS_Dummy_Loop::synchronizedDummyLoop( // old policy instead of the latest one. const auto policyUpdatedForTime = [this](scalar_t time) { constexpr scalar_t tol = - 0.1; // policy must start within this fraction of dt + 1.0; // policy must start within this fraction of dt + // change from 0.1 to 1 as more lenient synchronization requirements return mrt_.updatePolicy() && std::abs(mrt_.getPolicy().timeTrajectory_.front() - time) < (tol / mpcDesiredFrequency_); @@ -119,6 +120,11 @@ void MRT_ROS_Dummy_Loop::synchronizedDummyLoop( // Wait for the policy to be updated while (!policyUpdatedForTime(currentObservation.time) && rclcpp::ok()) { mrt_.spinMRT(); + // reset current observation time in case of large gap between mrt policy update time, need to better modify later + if (std::abs(mrt_.getPolicy().timeTrajectory_.front() - currentObservation.time) >= 10.0 / mrtDesiredFrequency_){ + currentObservation.time = mrt_.getPolicy().timeTrajectory_.front(); + std::cout << "[Warning] The MPC time and MRT time interval exceeds the MRT period. Force MRT to synchronize with MPC.\n"; + } } std::cout << "<<< New MPC policy starting at " << mrt_.getPolicy().timeTrajectory_.front() << "\n"; diff --git a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp index 73f959434..78ff536b8 100644 --- a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp +++ b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp @@ -99,7 +99,7 @@ void MRT_ROS_Interface::setCurrentObservation( lk.unlock(); msgReady_.notify_one(); #else - mpcObservationPublisher_.publish(mpcObservationMsg_); + mpcObservationPublisher_->publish(mpcObservationMsg_); #endif } diff --git a/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp b/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp index 3af495a4b..7ac9da2c0 100644 --- a/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp +++ b/ocs2_ros_interfaces/src/synchronized_module/RosReferenceManager.cpp @@ -27,6 +27,9 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ +#include +#include + #include "ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h" #include "ocs2_ros_interfaces/common/RosMsgConversions.h" @@ -74,4 +77,65 @@ void RosReferenceManager::subscribe(const rclcpp::Node::SharedPtr& node) { topicPrefix_ + "_mpc_target", 1, targetTrajectoriesCallback); } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void RosReferenceManager::subscribe_act(const rclcpp::Node::SharedPtr& node, PinocchioInterface pinocchioInterface, int eeFrameId, const std::string& armSide) { + node_ = node; + + // ModeSchedule + auto modeScheduleCallback = [this](const ocs2_msgs::msg::ModeSchedule& msg) { + auto modeSchedule = ros_msg_conversions::readModeScheduleMsg(msg); + referenceManagerPtr_->setModeSchedule(std::move(modeSchedule)); + }; + modeScheduleSubscriber_ = + node_->create_subscription( + topicPrefix_ + "_mode_schedule", 1, modeScheduleCallback); + + // get robot model for forward kinematics + auto model = pinocchioInterface.getModel(); + + // TargetTrajectories + auto actTargetTrajectoriesCallback = [this, model, eeFrameId, armSide](const std_msgs::msg::Float32MultiArray& msg) { + // msg = [ left_arm_qpos (6), # absolute joint position + // left_gripper_position (1), # normalized gripper position (0: close, 1: open) + // right_arm_qpos (6), # absolute joint position + // right_gripper_qpos (1), # normalized gripper position (0: close, 1: open) + // ros_time_stamp ] # rospy.Time.now().to_sec() + auto actTargetQpos = ros_msg_conversions::readActTargetQposMsg(msg); + // Split into left arm and right arm + Eigen::VectorXd armTargetQpos; + if (armSide == "LEFT") { + armTargetQpos = actTargetQpos.head(6); // left_arm_qpos + } else if (armSide == "RIGHT") { + armTargetQpos = actTargetQpos.segment(7, 6); // right_arm_qpos + } else { + std::cerr << "[Error] Invalid armSide parameter " << armSide << ". Use LEFT or RIGHT.\n"; + return; + } + + pinocchio::Data data(model); + pinocchio::forwardKinematics(model, data, armTargetQpos); + pinocchio::updateFramePlacements(model, data); + + // calculate end frame quaternion + Eigen::Quaterniond quaternion(data.oMf[eeFrameId].rotation()); + const vector_t target = (vector_t(7) << data.oMf[eeFrameId].translation(), quaternion.coeffs()).finished(); + + // set time stamp + const scalar_array_t desiredTimeTrajectory{actTargetQpos[14]}; + // set end frame position and quaternion + const vector_array_t desiredStateTrajectory{target}; + // set input trajectory all zero + const vector_array_t desiredInputTrajectory{vector_t::Zero(model.nv)}; + TargetTrajectories targetTrajectories(desiredTimeTrajectory, desiredStateTrajectory, desiredInputTrajectory); + + referenceManagerPtr_->setTargetTrajectories(std::move(targetTrajectories)); + }; + + actTargetTrajectoriesSubscriber_ = + node_->create_subscription( + "/act_aloha_target_qpos", 1, actTargetTrajectoriesCallback); +} + } // namespace ocs2