From 77bd6391c5382c4b7bc4eb3aab03a4b9e2c0e486 Mon Sep 17 00:00:00 2001 From: luzia Date: Mon, 30 Oct 2023 19:44:10 +0100 Subject: [PATCH 1/5] start integration of constraints --- .../models/inequalities/RadialConstraints.py | 2 +- ros_bridge/README.md | 1 + .../config/boxer_mpc_config.yaml | 10 +- .../robotmpcs_ros/launch/lidar_merger.launch | 10 ++ .../src/robotmpcs_ros/src/mpc_planner_node | 122 ++++++++++++++++-- 5 files changed, 132 insertions(+), 13 deletions(-) create mode 100644 ros_bridge/src/robotmpcs_ros/launch/lidar_merger.launch diff --git a/robotmpcs/models/inequalities/RadialConstraints.py b/robotmpcs/models/inequalities/RadialConstraints.py index 4785f70..d781709 100644 --- a/robotmpcs/models/inequalities/RadialConstraints.py +++ b/robotmpcs/models/inequalities/RadialConstraints.py @@ -19,6 +19,6 @@ def set_parameters(self, ParamMap, npar): def eval_constraint(self, z, p): - ineqs = self.eval_obstacleDistances(z,p,j) + ineqs = self.eval_obstacleDistances(z,p) return ineqs diff --git a/ros_bridge/README.md b/ros_bridge/README.md index 10751fc..635667b 100644 --- a/ros_bridge/README.md +++ b/ros_bridge/README.md @@ -19,6 +19,7 @@ Install `robot_mpcs` globally (or in the virtual environment if you use ros insi ```bash pip install -e . ``` +ira_laser_tools Build catkin workspace ```bash diff --git a/ros_bridge/src/robotmpcs_ros/config/boxer_mpc_config.yaml b/ros_bridge/src/robotmpcs_ros/config/boxer_mpc_config.yaml index 4e7f6bc..e6961b8 100644 --- a/ros_bridge/src/robotmpcs_ros/config/boxer_mpc_config.yaml +++ b/ros_bridge/src/robotmpcs_ros/config/boxer_mpc_config.yaml @@ -8,16 +8,20 @@ mpc: interval: 1 initialization: previous_plan constraints: - - RadialConstraints + - LinearConstraints - SelfCollisionAvoidanceConstraints - JointLimitConstraints - InputLimitConstraints + objectives: + - 'GoalReaching' + - 'ConstraintAvoidance' weights: - w: 1.0 + wgoal: 1.0 wvel: [0.0, 0.0, 0.0, 1, 0.01] ws: 1e10 wu: 0.01 wobst: 0.5 + wconstr: [0.0, 0.0, 0.0, 0.0] number_obstacles: 1 control_mode: "acc" robot: @@ -29,3 +33,5 @@ robot: root_link: base_link end_link: ee_link base_type: diffdrive +example: + debug: False diff --git a/ros_bridge/src/robotmpcs_ros/launch/lidar_merger.launch b/ros_bridge/src/robotmpcs_ros/launch/lidar_merger.launch new file mode 100644 index 0000000..5240d45 --- /dev/null +++ b/ros_bridge/src/robotmpcs_ros/launch/lidar_merger.launch @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/ros_bridge/src/robotmpcs_ros/src/mpc_planner_node b/ros_bridge/src/robotmpcs_ros/src/mpc_planner_node index 8e1fbf7..66fdee6 100755 --- a/ros_bridge/src/robotmpcs_ros/src/mpc_planner_node +++ b/ros_bridge/src/robotmpcs_ros/src/mpc_planner_node @@ -1,15 +1,19 @@ #!/usr/bin/env python3 -from typing import Union +from typing import Union, Tuple,List import numpy as np from std_msgs.msg import Float64MultiArray +from sensor_msgs.msg import PointCloud2 import rospkg +import sys import rospy +import sensor_msgs.point_cloud2 as pc2 from geometry_msgs.msg import Pose, Twist from tf.transformations import euler_from_quaternion from nav_msgs.msg import Odometry from robotmpcs.planner.mpcPlanner import MPCPlanner from mpscenes.goals.goal_composition import GoalComposition from mpscenes.obstacles.sphere_obstacle import SphereObstacle +from robotmpcs.utils.free_space_decomposition import FreeSpaceDecomposition def get_rotation(pose: Pose) -> float: orientation_q = pose.orientation @@ -40,6 +44,7 @@ class MPCNode(): def init_scenario(self): self._goal = None + self._lidar_pointcloud = None obst1Dict = { "type": "sphere", "geometry": {"position": [400.0, -1.5, 0.0], "radius": 1.0}, @@ -69,6 +74,9 @@ class MPCNode(): rospack = rospkg.RosPack() self._solver_directory = rospack.get_path("robotmpcs_ros") + "/solvers/" self._config = rospy.get_param('/mpc') + self._N = self._config['time_horizon'] + self._n_obstacles = self._config['number_obstacles'] + self._fsd = FreeSpaceDecomposition(number_constraints=self._n_obstacles, max_radius=5) self._planner = MPCPlanner( self._robot_type, self._solver_directory, @@ -77,13 +85,57 @@ class MPCNode(): self._planner.reset() def set_mpc_parameter(self): - self._planner.setObstacles(self._obstacles, self._r_body) - if hasattr(self, '_limits'):#todo also check if they were included in solver - self._planner.setJointLimits(np.transpose(self._limits)) - if hasattr(self, '_limits_vel'): - self._planner.setVelLimits(np.transpose(self._limits_vel)) - if hasattr(self, '_limits_u'): - self._planner.setInputLimits(np.transpose(self._limits_u)) + constraints = self._config['constraints'] + objectives = self._config['objectives'] + + for objective in objectives: + if objective == 'GoalReaching': + try: + self._planner.setGoalReaching(self._goal) + except AttributeError: + print('The required attributes for setting ' + objective + ' are not defined') + elif objective == 'ConstraintAvoidance': + try: + self._planner.setConstraintAvoidance() + except KeyError: + print('The required attributes for setting ' + objective + ' are not defined in the config file') + else: + print('No function to set the parameters for this objective is defined') + + + for constraint in constraints: + if constraint == 'JointLimitConstraints': + try: + self._planner.setJointLimits(np.transpose(self._limits)) + except AttributeError: + print('The required attributes for setting ' + constraint + ' are not defined') + elif constraint == 'VelLimitConstraints': + try: + self._planner.setVelLimits(np.transpose(self._limits_vel)) + except AttributeError: + print('The required attributes for setting ' + constraint + ' are not defined') + elif constraint == 'InputLimitConstraints': + try: + self._planner.setInputLimits(np.transpose(self._limits_u)) + except AttributeError: + print('The required attributes for setting ' + constraint + ' are not defined') + elif constraint == 'LinearConstraints': + try: + self._planner.setLinearConstraints(self._lin_constr, self._r_body) + except AttributeError: + print('The required attributes for setting ' + constraint + ' are not defined') + elif constraint == 'RadialConstraints': + try: + self._planner.setRadialConstraints(self._obstacles, self._r_body) + except AttributeError: + print('The required attributes for setting ' + constraint + ' are not defined') + elif constraint == 'SelfCollisionAvoidanceConstraints': + try: + self._planner.setSelfCollisionAvoidanceConstraints(self._r_body) + except AttributeError: + print('The required attributes for setting ' + constraint + ' are not defined') + else: + print('No function to set the parameters for this constraint type is defined') def establish_ros_connections(self): self._cmd_pub = rospy.Publisher( @@ -96,6 +148,10 @@ class MPCNode(): "/mpc/goal", Float64MultiArray, self._goal_cb ) + self._lidar_sub = rospy.Subscriber( + "/merged_cloud", PointCloud2, self._lidar_cb + ) + def _goal_cb(self, goal_msg: Float64MultiArray): goal_position = goal_msg.data if len(goal_position) != 2: @@ -113,7 +169,7 @@ class MPCNode(): } } self._goal = GoalComposition(name="goal1", content_dict=goal_dict) - self._planner.setGoal(self._goal) + self._planner.setGoalReaching(self._goal) def _odom_cb(self, odom_msg: Odometry): @@ -128,6 +184,37 @@ class MPCNode(): odom_msg.twist.twist.angular.z, ]) + def _lidar_cb(self, msg: PointCloud2): + point_cloud_array = [] + + # Use pc2.read_points to iterate through the PointCloud2 message + for point in pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True): + point_cloud_array.append([point[0], point[1], point[2]]) # Extract x, y, z coordinates + + # Now, point_cloud_array contains the array of points from the PointCloud2 message + # Use this array for further processing or manipulation + # Example: print the first 5 points + self._lidar_pointcloud = np.array(point_cloud_array) + + + def compute_constraints(self, robot_state: np.ndarray, point_cloud: np.ndarray) -> Tuple[List, List]: + """ + Computes linear constraints given a pointcloud as numpy array. + The seed point is the robot_state. + """ + angle = robot_state[2] + rot_matrix = np.array([ + [np.cos(angle), -np.sin(angle)], + [np.sin(angle), np.cos(angle)], + ]) + + position_lidar = np.dot(rot_matrix, np.array([0.4, 0.0])) + robot_state[0:2] + lidar_position = np.array([position_lidar[0], position_lidar[1], 0.02]) + + self._fsd.set_position(lidar_position) + self._fsd.compute_constraints(point_cloud) + return list(self._fsd.asdict().values()), self._fsd.constraints() + def act(self): vel_action = self._action * self._dt + self._qudot cmd_msg = Twist() @@ -138,8 +225,22 @@ class MPCNode(): def run(self): while not rospy.is_shutdown(): + if self._lidar_pointcloud is not None: + linear_constraints = [] + halfplanes = [] + for j in range(self._N): + if not self._output is None and self._exitflag >= 0: + key = "x{:02d}".format(j + 1) + ref_q = self._output[key][0:3] + else: + ref_q = self._q + linear_constraints_j, halfplanes_j = self.compute_constraints(ref_q, self._lidar_pointcloud) + linear_constraints.append(linear_constraints_j) + halfplanes.append(halfplanes_j) + + self._planner.setLinearConstraints(linear_constraints, r_body=self._r_body) if self._goal: - self._action, _ = self._planner.computeAction(self._q, self._qdot, self._qudot) + self._action, self._output, self._exitflag = self._planner.computeAction(self._q, self._qdot, self._qudot) #self._action = np.array([1.0, 0.2]) rospy.loginfo(self._action) self.act() @@ -148,6 +249,7 @@ class MPCNode(): if __name__ == "__main__": try: mpc_node = MPCNode() + mpc_node._output: Union[dict, None] = None mpc_node.run() except rospy.ROSInterruptException: pass From b7558a65ab69e945dae23be4b170d1cbb2498cd8 Mon Sep 17 00:00:00 2001 From: luzia Date: Tue, 31 Oct 2023 14:55:43 +0100 Subject: [PATCH 2/5] debugging transformation --- examples/boxer_example.py | 10 +- examples/config/boxerMpc.yaml | 2 +- robotmpcs/planner/mpcPlanner.py | 4 +- .../config/boxer_mpc_config.yaml | 3 +- .../src/robotmpcs_ros/rviz/boxer_example.rviz | 339 ++++++++++++++++++ .../src/robotmpcs_ros/src/mpc_planner_node | 100 +++++- .../src/robotmpcs_ros/utils/ros_visuals.py | 238 ++++++++++++ 7 files changed, 679 insertions(+), 17 deletions(-) create mode 100644 ros_bridge/src/robotmpcs_ros/rviz/boxer_example.rviz create mode 100644 ros_bridge/src/robotmpcs_ros/utils/ros_visuals.py diff --git a/examples/boxer_example.py b/examples/boxer_example.py index 5a75eed..1644a13 100644 --- a/examples/boxer_example.py +++ b/examples/boxer_example.py @@ -217,11 +217,11 @@ def run(self): # ---END: Visualizations--- def check_goal_reaching(self, ob): - primary_goal = self._goal.primary_goal() - goal_dist = np.linalg.norm(ob['robot_0']['joint_state']['position'][:2] - primary_goal.position()) # todo remove hard coded dimension, replace it with fk instead - if goal_dist <= primary_goal.epsilon(): - return True - return False + primary_goal = self._goal.primary_goal() + goal_dist = np.linalg.norm(ob['robot_0']['joint_state']['position'][:2] - primary_goal.position()) # todo remove hard coded dimension, replace it with fk instead + if goal_dist <= primary_goal.epsilon(): + return True + return False def main(): boxer_example = BoxerMpcExample(sys.argv[1]) diff --git a/examples/config/boxerMpc.yaml b/examples/config/boxerMpc.yaml index 5f089f1..0d15ba9 100644 --- a/examples/config/boxerMpc.yaml +++ b/examples/config/boxerMpc.yaml @@ -15,7 +15,7 @@ mpc: - 'GoalReaching' - 'ConstraintAvoidance' weights: - w: 1.0 + wgoal: 1.0 wvel: [0.0, 0.0, 0.0, 1, 0.01] ws: 1e10 wu: 0.01 diff --git a/robotmpcs/planner/mpcPlanner.py b/robotmpcs/planner/mpcPlanner.py index 7f72962..b450317 100644 --- a/robotmpcs/planner/mpcPlanner.py +++ b/robotmpcs/planner/mpcPlanner.py @@ -92,7 +92,7 @@ def reset(self): for i in range(self._config.time_horizon): self._params[ [self._npar * i + val for val in self._paramMap["wgoal"]] - ] = self._config.weights["w"] + ] = self._config.weights["wgoal"] # for j, val in enumerate(self._paramMap["wvel"]): # self._params[[self._npar * i + val]] = self._config.weights["wvel"][j] self._params[ @@ -199,8 +199,10 @@ def setGoalReaching(self, goal): for j in range(self.m()): if j >= len(goal.primary_goal().position()): position = 0 + print('no goal') else: position = goal.primary_goal().position()[j] + print('setting goal') self._params[self._npar * i + self._paramMap["goal"][j]] = position def setConstraintAvoidance(self): diff --git a/ros_bridge/src/robotmpcs_ros/config/boxer_mpc_config.yaml b/ros_bridge/src/robotmpcs_ros/config/boxer_mpc_config.yaml index e6961b8..cc2c95a 100644 --- a/ros_bridge/src/robotmpcs_ros/config/boxer_mpc_config.yaml +++ b/ros_bridge/src/robotmpcs_ros/config/boxer_mpc_config.yaml @@ -14,10 +14,9 @@ mpc: - InputLimitConstraints objectives: - 'GoalReaching' - - 'ConstraintAvoidance' weights: wgoal: 1.0 - wvel: [0.0, 0.0, 0.0, 1, 0.01] + wvel: [0.0, 0.0, 0.0, 0.0, 0.0] ws: 1e10 wu: 0.01 wobst: 0.5 diff --git a/ros_bridge/src/robotmpcs_ros/rviz/boxer_example.rviz b/ros_bridge/src/robotmpcs_ros/rviz/boxer_example.rviz new file mode 100644 index 0000000..fd7732f --- /dev/null +++ b/ros_bridge/src/robotmpcs_ros/rviz/boxer_example.rviz @@ -0,0 +1,339 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + Splitter Ratio: 0.5 + Tree Height: 719 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + cart_sensor: + Alpha: 1 + Show Axes: false + Show Trail: false + chassis_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + drive_train_link: + Alpha: 1 + Show Axes: false + Show Trail: false + front_laser: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + front_realsense_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + mid_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + realsense_front_camera_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + realsense_front_camera_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + realsense_front_camera_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + realsense_front_camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + realsense_front_camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + realsense_front_camera_gazebo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + realsense_front_camera_infra1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + realsense_front_camera_infra1_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + realsense_front_camera_infra2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + realsense_front_camera_infra2_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + realsense_front_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + rear_sensor: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rotacastor_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rotacastor_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_plate_link: + Alpha: 1 + Show Axes: false + Show Trail: false + vecow_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wifi1_antenna_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wifi1_antenna_hinge_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wifi1_antenna_inner_segment: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wifi1_antenna_outer_segment: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wifi2_antenna_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wifi2_antenna_hinge_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wifi2_antenna_inner_segment: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wifi2_antenna_outer_segment: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: /merged_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /mpc_planner_node/visuals + Name: MarkerArray + Namespaces: + "": true + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/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 + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 22.06820297241211 + 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 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5103980898857117 + Target Frame: + Yaw: 1.5003925561904907 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c70000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1848 + X: 72 + Y: 27 diff --git a/ros_bridge/src/robotmpcs_ros/src/mpc_planner_node b/ros_bridge/src/robotmpcs_ros/src/mpc_planner_node index 66fdee6..588f967 100755 --- a/ros_bridge/src/robotmpcs_ros/src/mpc_planner_node +++ b/ros_bridge/src/robotmpcs_ros/src/mpc_planner_node @@ -6,10 +6,14 @@ from sensor_msgs.msg import PointCloud2 import rospkg import sys import rospy +import tf +import tf2_ros import sensor_msgs.point_cloud2 as pc2 from geometry_msgs.msg import Pose, Twist from tf.transformations import euler_from_quaternion from nav_msgs.msg import Odometry +import tf2_sensor_msgs +from ros_bridge.src.robotmpcs_ros.utils.ros_visuals import ROSMarkerPublisher from robotmpcs.planner.mpcPlanner import MPCPlanner from mpscenes.goals.goal_composition import GoalComposition from mpscenes.obstacles.sphere_obstacle import SphereObstacle @@ -34,6 +38,7 @@ class MPCNode(): def __init__(self): rospy.init_node("mpc_node") + self.tf_buffer = tf2_ros.Buffer() self.establish_ros_connections() self._dt = rospy.get_param('/mpc/time_step') self._rate = rospy.Rate(1/self._dt) @@ -41,6 +46,11 @@ class MPCNode(): self.init_planner() self.set_mpc_parameter() self.init_arrays() + self.init_visuals() + self.listener = tf.TransformListener() + self._translation = [0,0,0] + self._rotation= [0,0,0,1] + def init_scenario(self): self._goal = None @@ -61,7 +71,18 @@ class MPCNode(): [-10, 10], [-10, 10], ]) + def init_visuals(self): + self.constr_color = 0 + + self.visuals = ROSMarkerPublisher('/mpc_planner_node/visuals', 100) + + self.visual_constraints = self.visuals.get_line('odom') + self.visual_constraints.set_scale(0.05) + self.visual_constraints.set_color(self.constr_color) + self.visual_goal = self.visuals.get_circle('odom') + self.visual_goal.set_scale(0.4,0.4,0.01) + self.visual_goal.set_color(1) def init_arrays(self): self._action = np.zeros(2) @@ -185,15 +206,50 @@ class MPCNode(): ]) def _lidar_cb(self, msg: PointCloud2): - point_cloud_array = [] + print(msg.header.frame_id) + + while True: + point_cloud_array = [] + try: + # Transform the PointCloud2 to the desired frame + + transform = self.tf_buffer.lookup_transform('odom', msg.header.frame_id, rospy.Time(0)) + + #self._translation = transform.transform.translation + print('transform') + print(transform) + break + + #cloud_points = np.array(list(pc2.read_points(msg))) + + #transformed_points = np.dot(cloud_points[:, :3], np.transpose([transform.transform.rotation.x, + # transform.transform.rotation.y, + # transform.transform.rotation.z, + # transform.transform.rotation.w])) + #transformed_points += [transform.transform.translation.x, + # transform.transform.translation.y, + # transform.transform.translation.z] + #print('transformed') + + + except Exception as e: + rospy.logwarn(f"Could not transform the lidar PointCloud {e}") + # Print additional information for debugging + print(f"Source frame: {msg.header.frame_id}") + print(f"Target frame: odom") + print(f"TF tree:\n{self.tf_buffer.all_frames_as_string()}") + self._rate.sleep() + + # Use pc2.read_points to iterate through the PointCloud2 message - for point in pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True): - point_cloud_array.append([point[0], point[1], point[2]]) # Extract x, y, z coordinates # Now, point_cloud_array contains the array of points from the PointCloud2 message # Use this array for further processing or manipulation # Example: print the first 5 points + transformed_cloud = tf2_sensor_msgs.do_transform_cloud(msg, transform) + for point in pc2.read_points(transformed_cloud, field_names=("x", "y", "z"), skip_nans=True): + point_cloud_array.append([point[0], point[1], point[2]]) # Extract x, y, z coordinates self._lidar_pointcloud = np.array(point_cloud_array) @@ -208,11 +264,13 @@ class MPCNode(): [np.sin(angle), np.cos(angle)], ]) - position_lidar = np.dot(rot_matrix, np.array([0.4, 0.0])) + robot_state[0:2] - lidar_position = np.array([position_lidar[0], position_lidar[1], 0.02]) + position_lidar = np.dot(rot_matrix, np.array([0.4, 0.0])) + self._translation[:2] + + lidar_position = np.array([position_lidar[0], position_lidar[1], self._translation[2]]) self._fsd.set_position(lidar_position) - self._fsd.compute_constraints(point_cloud) + if len(point_cloud)>0: + self._fsd.compute_constraints(point_cloud) return list(self._fsd.asdict().values()), self._fsd.constraints() def act(self): @@ -225,8 +283,8 @@ class MPCNode(): def run(self): while not rospy.is_shutdown(): + linear_constraints = [] if self._lidar_pointcloud is not None: - linear_constraints = [] halfplanes = [] for j in range(self._N): if not self._output is None and self._exitflag >= 0: @@ -241,11 +299,37 @@ class MPCNode(): self._planner.setLinearConstraints(linear_constraints, r_body=self._r_body) if self._goal: self._action, self._output, self._exitflag = self._planner.computeAction(self._q, self._qdot, self._qudot) - #self._action = np.array([1.0, 0.2]) rospy.loginfo(self._action) self.act() + if self.check_goal_reaching(self._q): + print('GOAL REACHED') + + self.visualize(linear_constraints) self._rate.sleep() + def visualize(self,linear_constraints): + if len(linear_constraints)>0: + for constraint in linear_constraints[0]: + self.visual_constraints.add_constraint_line(constraint[:2], -constraint[-1], length=10.0) + + if self._goal is not None: + goal_pose = Pose() + print(self._goal.primary_goal()) + goal_pose.position.x = self._goal.primary_goal().position()[0] + goal_pose.position.y = self._goal.primary_goal().position()[1] + goal_pose.position.z = 0.01 + self.visual_goal.add_marker(goal_pose) + self.visuals.publish() + + def check_goal_reaching(self, q): + if self._goal is not None: + primary_goal = self._goal.primary_goal() + goal_dist = np.linalg.norm(q[:2] - primary_goal.position()) # todo remove hard coded dimension, replace it with fk instead + if goal_dist <= primary_goal.epsilon(): + return True + return False + return False + if __name__ == "__main__": try: mpc_node = MPCNode() diff --git a/ros_bridge/src/robotmpcs_ros/utils/ros_visuals.py b/ros_bridge/src/robotmpcs_ros/utils/ros_visuals.py new file mode 100644 index 0000000..c1371c7 --- /dev/null +++ b/ros_bridge/src/robotmpcs_ros/utils/ros_visuals.py @@ -0,0 +1,238 @@ +import rospy +from visualization_msgs.msg import MarkerArray, Marker +from geometry_msgs.msg import Pose, Point +from copy import deepcopy + +COLORS = [[153, 51, 102], [0, 255, 0], [254, 254, 0], [254, 0, 254], [0, 255, 255], [255, 153, 0]] + +class ROSMarkerPublisher: + + """ + Collects ROS Markers + """ + + def __init__(self, topic, max_size): + self.pub_ = rospy.Publisher(topic, MarkerArray, queue_size=1) + self.marker_list_ = MarkerArray() + + self.clear_all() + + self.ros_markers_ = [] + + self.topic_ = topic + self.max_size_ = max_size + self.id_ = 0 + self.prev_id_ = 0 + + def clear_all(self): + remove_marker = Marker() + remove_marker.action = remove_marker.DELETEALL + self.marker_list_.markers = [] + self.marker_list_.markers.append(remove_marker) + self.pub_.publish(self.marker_list_) + + def __del__(self): + self.clear_all() + + def publish(self): + # print('DEBUG: Publishing markers') + + remove_marker = Marker() + remove_marker.action = remove_marker.DELETE + + if self.prev_id_ > self.id_: + for i in range(self.id_, self.prev_id_): + remove_marker.id = i + self.marker_list_.markers.append(deepcopy(remove_marker)) + + # Define stamp properties + for marker in self.marker_list_.markers: + marker.header.stamp = rospy.Time.now() + + # Publish + # print(self.marker_list_.markers) + #print('Publishing {} markers'.format(len(self.marker_list_.markers))) + self.pub_.publish(self.marker_list_) + + # Reset + self.marker_list_.markers = [] + + self.prev_id_ = deepcopy(self.id_) + self.id_ = 0 + + def get_cube(self, frame_id="map"): + new_marker = ROSMarker(1, frame_id, self) + return self.get_point_marker(new_marker) + + def get_sphere(self, frame_id="map"): + new_marker = ROSMarker(2, frame_id, self) + return self.get_point_marker(new_marker) + + def get_cylinder(self, frame_id="map"): + new_marker = ROSMarker(3, frame_id, self) + return self.get_point_marker(new_marker) + + def get_circle(self, frame_id="map"): + new_marker = ROSMarker(3, frame_id, self) + new_marker.set_scale(1., 1., 0.01) + return self.get_point_marker(new_marker) + + def get_line(self, frame_id="map"): + new_marker = ROSLineMarker(frame_id, self) + return self.get_point_marker(new_marker) + + def get_point_marker(self, new_marker): + self.ros_markers_.append(new_marker) + return new_marker # Gives the option to modify it + + def add_marker_(self, new_marker): + self.marker_list_.markers.append(new_marker) + + def get_id_(self): + cur_id = deepcopy(self.id_) + self.id_ += 1 + return cur_id + +class ROSMarker: + + def __init__(self, type, frame_id, ros_marker_publisher): + self.marker_ = Marker() + self.marker_.header.frame_id = frame_id + self.type_ = type + self.marker_.type = self.type_ + self.ros_marker_publisher_ = ros_marker_publisher + + self.set_color(0) + self.set_scale(1, 1, 1) + + def set_scale(self, x, y, z): + self.marker_.scale.x = x + self.marker_.scale.y = y + self.marker_.scale.z = z + + def set_scale_all(self, all=1.0): + self.marker_.scale.x = all + self.marker_.scale.y = all + self.marker_.scale.z = all + + def set_color(self, int_val, alpha=1.0): + red, green, blue = get_viridis_color(int_val) + self.marker_.color.r = red + self.marker_.color.g = green + self.marker_.color.b = blue + self.marker_.color.a = alpha + + def set_lifetime(self, lifetime): + self.marker_.lifetime = rospy.Time(lifetime) + + def add_marker(self, pose): + self.marker_.id = self.ros_marker_publisher_.get_id_() + self.marker_.pose = pose + self.marker_.pose.orientation.x = 0 + self.marker_.pose.orientation.y = 0 + self.marker_.pose.orientation.z = 0 + self.marker_.pose.orientation.w = 1 + self.ros_marker_publisher_.add_marker_(deepcopy(self.marker_)) # Note the deepcopy + +class ROSLineMarker: + + def __init__(self, frame_id, ros_marker_publisher): + self.marker_ = Marker() + self.marker_.header.frame_id = frame_id + self.marker_.type = 5 + self.ros_marker_publisher_ = ros_marker_publisher + + self.marker_.scale.x = 0.25 + self.marker_.pose.orientation.x = 0 + self.marker_.pose.orientation.y = 0 + self.marker_.pose.orientation.z = 0 + self.marker_.pose.orientation.w = 1 + self.set_color(1) + + # Ax <= b (visualized) + def add_constraint_line(self, A, b, length=10.0): + a1 = A[0] + a2 = A[1] + b = float(b) + p1 = Point() + p2 = Point() + if abs(a1) < 0.01 and abs(a2) >= 0.01: + p1.x = -length + p1.y = (b + a1 * length) / float(a2) + p1.z = 0. + + p2.x = length + p2.y = (b - a1 * length) / float(a2) + p2.z = 0. + elif(abs(a1) >= 0.01): + p1.y = -length + p1.x = (b + a2 * length) / float(a1) + p1.z = 0. + + p2.y = length + p2.x = (b - a2 * length) / float(a1) + p2.z = 0. + else: + raise Exception("visualized constrained is zero") + + p1.x = min(max(p1.x, -1e5), 1e5) + p2.x = min(max(p2.x, -1e5), 1e5) + p1.y = min(max(p1.y, -1e5), 1e5) + p2.y = min(max(p2.y, -1e5), 1e5) + + self.add_line(p1, p2) + + def add_line_from_poses(self, pose_one, pose_two): + p1 = Point() + p1.x = pose_one.position.x + p1.y = pose_one.position.y + p1.z = pose_one.position.z + + p2 = Point() + p2.x = pose_two.position.x + p2.y = pose_two.position.y + p2.z = pose_two.position.z + + self.add_line(p1, p2) + + def add_line(self, point_one, point_two): + self.marker_.id = self.ros_marker_publisher_.get_id_() + + self.marker_.points.append(point_one) + self.marker_.points.append(point_two) + + self.ros_marker_publisher_.add_marker_(deepcopy(self.marker_)) + + self.marker_.points = [] + + def set_color(self, int_val, alpha=1.0): + red, green, blue = get_viridis_color(int_val) + self.marker_.color.r = red + self.marker_.color.g = green + self.marker_.color.b = blue + self.marker_.color.a = alpha + + def set_scale(self, thickness): + self.marker_.scale.x = thickness + + +def get_viridis_color(select): + # Obtained from https://waldyrious.net/viridis-palette-generator/ + viridis_vals = [253, 231, 37, 234, 229, 26, 210, 226, 27, 186, 222, 40, 162, 218, 55, 139, 214, 70, 119, 209, 83, 99, + 203, 95, 80, 196, 106, 63, 188, 115, 49, 181, 123, 38, 173, 129, 33, 165, 133, 30, 157, 137, 31, 148, 140, 34, 140, + 141, 37, 131, 142, 41, 123, 142, 44, 115, 142, 47, 107, 142, 51, 98, 141, 56, 89, 140] + + VIRIDIS_COLORS = 20 + select %= VIRIDIS_COLORS # only 20 values specified + + # Invert the color range + select = VIRIDIS_COLORS - 1 - select + red = viridis_vals[select * 3 + 0] + green = viridis_vals[select * 3 + 1] + blue = viridis_vals[select * 3 + 2] + + red /= 256.0 + green /= 256.0 + blue /= 256.0 + + return red, green, blue \ No newline at end of file From 22b6c53ec706ca1feb2ea45556a76ee121ff67ad Mon Sep 17 00:00:00 2001 From: luzia Date: Tue, 31 Oct 2023 18:44:11 +0100 Subject: [PATCH 3/5] linear constraints visualizations match lidar points --- .../src/robotmpcs_ros/rviz/boxer_example.rviz | 58 ++++++------ .../src/robotmpcs_ros/src/mpc_planner_node | 91 +++++++++---------- .../robotmpcs_ros/utils/transformations.py | 10 ++ 3 files changed, 84 insertions(+), 75 deletions(-) create mode 100644 ros_bridge/src/robotmpcs_ros/utils/transformations.py diff --git a/ros_bridge/src/robotmpcs_ros/rviz/boxer_example.rviz b/ros_bridge/src/robotmpcs_ros/rviz/boxer_example.rviz index fd7732f..fc08089 100644 --- a/ros_bridge/src/robotmpcs_ros/rviz/boxer_example.rviz +++ b/ros_bridge/src/robotmpcs_ros/rviz/boxer_example.rviz @@ -6,9 +6,9 @@ Panels: Expanded: - /Global Options1 - /Status1 - - /PointCloud21 + - /LaserScan1 Splitter Ratio: 0.5 - Tree Height: 719 + Tree Height: 746 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -26,7 +26,7 @@ Panels: - Class: rviz/Time Name: Time SyncMode: 0 - SyncSource: PointCloud2 + SyncSource: LaserScan Preferences: PromptSaveOnExit: true Toolbars: @@ -235,6 +235,14 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /mpc_planner_node/visuals + Name: MarkerArray + Namespaces: + "": true + Queue Size: 100 + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -243,7 +251,7 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz/LaserScan Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 @@ -251,26 +259,18 @@ Visualization Manager: Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 - Name: PointCloud2 + Name: LaserScan Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.05000000074505806 + Size (m): 0.10000000149011612 Style: Flat Squares - Topic: /merged_cloud + Topic: /front/scan Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /mpc_planner_node/visuals - Name: MarkerArray - Namespaces: - "": true - Queue Size: 100 - Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -291,7 +291,7 @@ Visualization Manager: X std deviation: 0.5 Y std deviation: 0.5 - Class: rviz/SetGoal - Topic: /move_base_simple/goal + Topic: /mpc/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point @@ -299,7 +299,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 22.06820297241211 + Distance: 46.13591384887695 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -307,25 +307,25 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 0 - Y: 0 - Z: 0 + X: 7.651804447174072 + Y: -1.4001134634017944 + Z: 2.442548990249634 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.5103980898857117 + Pitch: 0.5653980374336243 Target Frame: - Yaw: 1.5003925561904907 + Yaw: 1.59039306640625 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1016 + Height: 1043 Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c70000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000015600000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003c00000003efc0100000002fb0000000800540069006d00650100000000000003c0000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000002640000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -333,7 +333,7 @@ Window Geometry: Tool Properties: collapsed: false Views: - collapsed: false - Width: 1848 - X: 72 - Y: 27 + collapsed: true + Width: 960 + X: 2880 + Y: 450 diff --git a/ros_bridge/src/robotmpcs_ros/src/mpc_planner_node b/ros_bridge/src/robotmpcs_ros/src/mpc_planner_node index 588f967..9a201c2 100755 --- a/ros_bridge/src/robotmpcs_ros/src/mpc_planner_node +++ b/ros_bridge/src/robotmpcs_ros/src/mpc_planner_node @@ -2,18 +2,20 @@ from typing import Union, Tuple,List import numpy as np from std_msgs.msg import Float64MultiArray -from sensor_msgs.msg import PointCloud2 +from sensor_msgs.msg import PointCloud2, LaserScan import rospkg import sys import rospy import tf import tf2_ros +import math import sensor_msgs.point_cloud2 as pc2 -from geometry_msgs.msg import Pose, Twist +from geometry_msgs.msg import Pose, Twist, PoseStamped from tf.transformations import euler_from_quaternion from nav_msgs.msg import Odometry import tf2_sensor_msgs from ros_bridge.src.robotmpcs_ros.utils.ros_visuals import ROSMarkerPublisher +from ros_bridge.src.robotmpcs_ros.utils.transformations import quaternion_to_rotation_matrix from robotmpcs.planner.mpcPlanner import MPCPlanner from mpscenes.goals.goal_composition import GoalComposition from mpscenes.obstacles.sphere_obstacle import SphereObstacle @@ -38,7 +40,7 @@ class MPCNode(): def __init__(self): rospy.init_node("mpc_node") - self.tf_buffer = tf2_ros.Buffer() + self._transform = None self.establish_ros_connections() self._dt = rospy.get_param('/mpc/time_step') self._rate = rospy.Rate(1/self._dt) @@ -47,9 +49,11 @@ class MPCNode(): self.set_mpc_parameter() self.init_arrays() self.init_visuals() + self.tf_buffer = tf2_ros.Buffer() self.listener = tf.TransformListener() - self._translation = [0,0,0] - self._rotation= [0,0,0,1] + + + def init_scenario(self): @@ -166,15 +170,15 @@ class MPCNode(): "/odometry/filtered", Odometry, self._odom_cb ) self._goal_sub = rospy.Subscriber( - "/mpc/goal", Float64MultiArray, self._goal_cb + "/mpc/goal", PoseStamped, self._goal_cb ) self._lidar_sub = rospy.Subscriber( - "/merged_cloud", PointCloud2, self._lidar_cb + "/front/scan", LaserScan, self._lidar_cb ) - def _goal_cb(self, goal_msg: Float64MultiArray): - goal_position = goal_msg.data + def _goal_cb(self, goal_msg: PoseStamped): + goal_position = [goal_msg.pose.position.x, goal_msg.pose.position.y] if len(goal_position) != 2: rospy.logwarn("Goal ignored because of dimension missmatch") goal_dict = { @@ -206,51 +210,42 @@ class MPCNode(): ]) def _lidar_cb(self, msg: PointCloud2): - print(msg.header.frame_id) + cloud_points = [] + angle = msg.angle_min - while True: - point_cloud_array = [] - try: - # Transform the PointCloud2 to the desired frame + for r in msg.ranges: + # Filter out invalid range values + if r > msg.range_min and r < msg.range_max: + x = r * math.cos(angle) + y = r * math.sin(angle) + z = 0.0 # Set z value as 0 (assuming a 2D plane) - transform = self.tf_buffer.lookup_transform('odom', msg.header.frame_id, rospy.Time(0)) + cloud_points.append([x, y, z]) + angle += msg.angle_increment - #self._translation = transform.transform.translation - print('transform') - print(transform) - break + self.listener.waitForTransform("/odom", msg.header.frame_id, rospy.Time(0), rospy.Duration(5)) + (self._translation, self._rotation) = self.listener.lookupTransform("/odom", msg.header.frame_id, rospy.Time(0)) - #cloud_points = np.array(list(pc2.read_points(msg))) - #transformed_points = np.dot(cloud_points[:, :3], np.transpose([transform.transform.rotation.x, - # transform.transform.rotation.y, - # transform.transform.rotation.z, - # transform.transform.rotation.w])) - #transformed_points += [transform.transform.translation.x, - # transform.transform.translation.y, - # transform.transform.translation.z] - #print('transformed') + if self._translation is not None: + cloud_points = np.array(cloud_points) + print(cloud_points.shape) + R = quaternion_to_rotation_matrix(self._rotation) - except Exception as e: - rospy.logwarn(f"Could not transform the lidar PointCloud {e}") - # Print additional information for debugging - print(f"Source frame: {msg.header.frame_id}") - print(f"Target frame: odom") - print(f"TF tree:\n{self.tf_buffer.all_frames_as_string()}") - self._rate.sleep() + transformed_points = np.dot(cloud_points[:, :3], R.T) + transformed_points += [self._translation[0], + self._translation[1], + self._translation[2]] + # print('transformed') + # Use pc2.read_points to iterate through the PointCloud2 message + # Now, point_cloud_array contains the array of points from the PointCloud2 message + # Use this array for further processing or manipulation + # Example: print the first 5 points - # Use pc2.read_points to iterate through the PointCloud2 message - - # Now, point_cloud_array contains the array of points from the PointCloud2 message - # Use this array for further processing or manipulation - # Example: print the first 5 points - transformed_cloud = tf2_sensor_msgs.do_transform_cloud(msg, transform) - for point in pc2.read_points(transformed_cloud, field_names=("x", "y", "z"), skip_nans=True): - point_cloud_array.append([point[0], point[1], point[2]]) # Extract x, y, z coordinates - self._lidar_pointcloud = np.array(point_cloud_array) + self._lidar_pointcloud = np.array(transformed_points) def compute_constraints(self, robot_state: np.ndarray, point_cloud: np.ndarray) -> Tuple[List, List]: @@ -258,15 +253,17 @@ class MPCNode(): Computes linear constraints given a pointcloud as numpy array. The seed point is the robot_state. """ + angle = robot_state[2] rot_matrix = np.array([ [np.cos(angle), -np.sin(angle)], [np.sin(angle), np.cos(angle)], ]) - position_lidar = np.dot(rot_matrix, np.array([0.4, 0.0])) + self._translation[:2] + position_lidar = self._translation[:2] - lidar_position = np.array([position_lidar[0], position_lidar[1], self._translation[2]]) + lidar_position = np.array([position_lidar[0], position_lidar[1], 0.0]) + print(position_lidar) self._fsd.set_position(lidar_position) if len(point_cloud)>0: @@ -282,6 +279,8 @@ class MPCNode(): self._qudot = vel_action def run(self): + + while not rospy.is_shutdown(): linear_constraints = [] if self._lidar_pointcloud is not None: diff --git a/ros_bridge/src/robotmpcs_ros/utils/transformations.py b/ros_bridge/src/robotmpcs_ros/utils/transformations.py new file mode 100644 index 0000000..00725f5 --- /dev/null +++ b/ros_bridge/src/robotmpcs_ros/utils/transformations.py @@ -0,0 +1,10 @@ +import numpy as np +def quaternion_to_rotation_matrix(q): + q /= np.linalg.norm(q) # Normalize quaternion + x, y, z, w = q + rotation_matrix = np.array([ + [1 - 2 * (y**2 + z**2), 2 * (x*y - w*z), 2 * (x*z + w*y)], + [2 * (x*y + w*z), 1 - 2 * (x**2 + z**2), 2 * (y*z - w*x)], + [2 * (x*z - w*y), 2 * (y*z + w*x), 1 - 2 * (x**2 + y**2)] + ]) + return rotation_matrix \ No newline at end of file From 3d63ed1e2f6a246b0babdee515dd01cb79533a9a Mon Sep 17 00:00:00 2001 From: luzia Date: Tue, 31 Oct 2023 19:35:21 +0100 Subject: [PATCH 4/5] add visualizations --- .../src/robotmpcs_ros/rviz/boxer_example.rviz | 163 +++++++++++++++++- .../src/robotmpcs_ros/src/mpc_planner_node | 47 +++-- 2 files changed, 197 insertions(+), 13 deletions(-) diff --git a/ros_bridge/src/robotmpcs_ros/rviz/boxer_example.rviz b/ros_bridge/src/robotmpcs_ros/rviz/boxer_example.rviz index fc08089..e85802c 100644 --- a/ros_bridge/src/robotmpcs_ros/rviz/boxer_example.rviz +++ b/ros_bridge/src/robotmpcs_ros/rviz/boxer_example.rviz @@ -7,6 +7,8 @@ Panels: - /Global Options1 - /Status1 - /LaserScan1 + - /TF1 + - /TF1/Frames1 Splitter Ratio: 0.5 Tree Height: 746 - Class: rviz/Selection @@ -271,6 +273,161 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true + - Class: rviz/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: false + base_footprint: + Value: false + base_link: + Value: false + cart_sensor: + Value: false + chassis_link: + Value: false + drive_train_link: + Value: false + front_laser: + Value: false + front_mount: + Value: false + front_realsense_mount_link: + Value: false + imu_link: + Value: false + lift_link: + Value: false + mid_mount: + Value: false + odom: + Value: true + realsense_front_camera_bottom_screw_frame: + Value: false + realsense_front_camera_color_frame: + Value: false + realsense_front_camera_color_optical_frame: + Value: false + realsense_front_camera_depth_frame: + Value: false + realsense_front_camera_depth_optical_frame: + Value: false + realsense_front_camera_gazebo_link: + Value: false + realsense_front_camera_infra1_frame: + Value: false + realsense_front_camera_infra1_optical_frame: + Value: false + realsense_front_camera_infra2_frame: + Value: false + realsense_front_camera_infra2_optical_frame: + Value: false + realsense_front_camera_link: + Value: false + rear_mount: + Value: false + rear_sensor: + Value: false + rotacastor_left_link: + Value: false + rotacastor_right_link: + Value: false + top_plate_link: + Value: false + vecow_base_link: + Value: false + wheel_left_link: + Value: false + wheel_right_link: + Value: false + wifi1_antenna_base_link: + Value: false + wifi1_antenna_hinge_link: + Value: false + wifi1_antenna_inner_segment: + Value: false + wifi1_antenna_outer_segment: + Value: false + wifi2_antenna_base_link: + Value: false + wifi2_antenna_hinge_link: + Value: false + wifi2_antenna_inner_segment: + Value: false + wifi2_antenna_outer_segment: + Value: false + Marker Alpha: 1 + Marker Scale: 15 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + odom: + base_link: + base_footprint: + {} + chassis_link: + drive_train_link: + rotacastor_left_link: + {} + rotacastor_right_link: + {} + wheel_left_link: + {} + wheel_right_link: + {} + front_laser: + {} + front_realsense_mount_link: + realsense_front_camera_bottom_screw_frame: + realsense_front_camera_link: + realsense_front_camera_color_frame: + realsense_front_camera_color_optical_frame: + {} + realsense_front_camera_depth_frame: + realsense_front_camera_depth_optical_frame: + {} + realsense_front_camera_gazebo_link: + {} + realsense_front_camera_infra1_frame: + realsense_front_camera_infra1_optical_frame: + {} + realsense_front_camera_infra2_frame: + realsense_front_camera_infra2_optical_frame: + {} + lift_link: + {} + rear_sensor: + {} + top_plate_link: + cart_sensor: + {} + front_mount: + {} + mid_mount: + {} + rear_mount: + {} + vecow_base_link: + wifi1_antenna_base_link: + wifi1_antenna_hinge_link: + wifi1_antenna_inner_segment: + {} + wifi1_antenna_outer_segment: + {} + wifi2_antenna_base_link: + wifi2_antenna_hinge_link: + wifi2_antenna_inner_segment: + {} + wifi2_antenna_outer_segment: + {} + imu_link: + {} + Update Interval: 0 + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -325,7 +482,7 @@ Window Geometry: Height: 1043 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000015600000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003c00000003efc0100000002fb0000000800540069006d00650100000000000003c0000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000002640000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000015600000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000006240000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -334,6 +491,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 960 - X: 2880 + Width: 1920 + X: 1920 Y: 450 diff --git a/ros_bridge/src/robotmpcs_ros/src/mpc_planner_node b/ros_bridge/src/robotmpcs_ros/src/mpc_planner_node index 9a201c2..e4b099f 100755 --- a/ros_bridge/src/robotmpcs_ros/src/mpc_planner_node +++ b/ros_bridge/src/robotmpcs_ros/src/mpc_planner_node @@ -88,6 +88,14 @@ class MPCNode(): self.visual_goal.set_scale(0.4,0.4,0.01) self.visual_goal.set_color(1) + self.visual_robot = self.visuals.get_circle('odom') + self.visual_robot.set_scale(0.7,0.7,0.01) + self.visual_robot.set_color(5) + + self.visual_plan_circle = self.visuals.get_circle('odom') + self.visual_plan_circle.set_scale(0.4,0.4,0.1) + self.visual_plan_circle.set_color(10,alpha=0.3) + def init_arrays(self): self._action = np.zeros(2) self._q = np.zeros(3) @@ -164,17 +172,17 @@ class MPCNode(): def establish_ros_connections(self): self._cmd_pub = rospy.Publisher( - "/boxer_velocity_controller/cmd_vel", Twist, queue_size=1 + "/boxer_velocity_controller/cmd_vel", Twist, queue_size=1, ) self._odom_sub = rospy.Subscriber( - "/odometry/filtered", Odometry, self._odom_cb + "/odometry/filtered", Odometry, self._odom_cb, queue_size=1, ) self._goal_sub = rospy.Subscriber( - "/mpc/goal", PoseStamped, self._goal_cb + "/mpc/goal", PoseStamped, self._goal_cb, queue_size=1, ) self._lidar_sub = rospy.Subscriber( - "/front/scan", LaserScan, self._lidar_cb + "/front/scan", LaserScan, self._lidar_cb, queue_size=1, ) def _goal_cb(self, goal_msg: PoseStamped): @@ -229,7 +237,7 @@ class MPCNode(): if self._translation is not None: cloud_points = np.array(cloud_points) - print(cloud_points.shape) + R = quaternion_to_rotation_matrix(self._rotation) @@ -263,7 +271,7 @@ class MPCNode(): position_lidar = self._translation[:2] lidar_position = np.array([position_lidar[0], position_lidar[1], 0.0]) - print(position_lidar) + self._fsd.set_position(lidar_position) if len(point_cloud)>0: @@ -289,6 +297,12 @@ class MPCNode(): if not self._output is None and self._exitflag >= 0: key = "x{:02d}".format(j + 1) ref_q = self._output[key][0:3] + + plan_pose = Pose() + plan_pose.position.x = ref_q[0] + plan_pose.position.y = ref_q[1] + plan_pose.position.z = 0.01 + self.visual_robot.add_marker(plan_pose) else: ref_q = self._q linear_constraints_j, halfplanes_j = self.compute_constraints(ref_q, self._lidar_pointcloud) @@ -296,9 +310,15 @@ class MPCNode(): halfplanes.append(halfplanes_j) self._planner.setLinearConstraints(linear_constraints, r_body=self._r_body) + print('linear constraints are set') + print(linear_constraints) if self._goal: self._action, self._output, self._exitflag = self._planner.computeAction(self._q, self._qdot, self._qudot) - rospy.loginfo(self._action) + rospy.loginfo(self._action) + rospy.loginfo(self._exitflag) + print(self._output) + + self.act() if self.check_goal_reaching(self._q): print('GOAL REACHED') @@ -308,16 +328,23 @@ class MPCNode(): def visualize(self,linear_constraints): if len(linear_constraints)>0: - for constraint in linear_constraints[0]: - self.visual_constraints.add_constraint_line(constraint[:2], -constraint[-1], length=10.0) + for constraint in linear_constraints: + self.visual_constraints.add_constraint_line(constraint[0][:2], -constraint[0][-1], length=10.0) + if self._goal is not None: goal_pose = Pose() - print(self._goal.primary_goal()) goal_pose.position.x = self._goal.primary_goal().position()[0] goal_pose.position.y = self._goal.primary_goal().position()[1] goal_pose.position.z = 0.01 self.visual_goal.add_marker(goal_pose) + + robot_pose = Pose() + robot_pose.position.x = self._q[0] + robot_pose.position.y = self._q[1] + robot_pose.position.z = 0.01 + self.visual_robot.add_marker(robot_pose) + self.visuals.publish() def check_goal_reaching(self, q): From a6b0d64c6d31b0cc268d6beb3456b9d47508ff12 Mon Sep 17 00:00:00 2001 From: luzia Date: Tue, 31 Oct 2023 19:53:33 +0100 Subject: [PATCH 5/5] change initial guess for mpc --- ros_bridge/src/robotmpcs_ros/config/boxer_mpc_config.yaml | 4 ++-- ros_bridge/src/robotmpcs_ros/src/mpc_planner_node | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ros_bridge/src/robotmpcs_ros/config/boxer_mpc_config.yaml b/ros_bridge/src/robotmpcs_ros/config/boxer_mpc_config.yaml index cc2c95a..8e45e7f 100644 --- a/ros_bridge/src/robotmpcs_ros/config/boxer_mpc_config.yaml +++ b/ros_bridge/src/robotmpcs_ros/config/boxer_mpc_config.yaml @@ -6,7 +6,7 @@ mpc: time_step: 0.1 slack: False interval: 1 - initialization: previous_plan + initialization: current_state constraints: - LinearConstraints - SelfCollisionAvoidanceConstraints @@ -19,7 +19,7 @@ mpc: wvel: [0.0, 0.0, 0.0, 0.0, 0.0] ws: 1e10 wu: 0.01 - wobst: 0.5 + wobst: 0.0 wconstr: [0.0, 0.0, 0.0, 0.0] number_obstacles: 1 control_mode: "acc" diff --git a/ros_bridge/src/robotmpcs_ros/src/mpc_planner_node b/ros_bridge/src/robotmpcs_ros/src/mpc_planner_node index e4b099f..d92559b 100755 --- a/ros_bridge/src/robotmpcs_ros/src/mpc_planner_node +++ b/ros_bridge/src/robotmpcs_ros/src/mpc_planner_node @@ -311,7 +311,7 @@ class MPCNode(): self._planner.setLinearConstraints(linear_constraints, r_body=self._r_body) print('linear constraints are set') - print(linear_constraints) + if self._goal: self._action, self._output, self._exitflag = self._planner.computeAction(self._q, self._qdot, self._qudot) rospy.loginfo(self._action)