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/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/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/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..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,18 +6,21 @@ mpc: time_step: 0.1 slack: False interval: 1 - initialization: previous_plan + initialization: current_state constraints: - - RadialConstraints + - LinearConstraints - SelfCollisionAvoidanceConstraints - JointLimitConstraints - InputLimitConstraints + objectives: + - 'GoalReaching' weights: - w: 1.0 - wvel: [0.0, 0.0, 0.0, 1, 0.01] + wgoal: 1.0 + 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" robot: @@ -29,3 +32,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/rviz/boxer_example.rviz b/ros_bridge/src/robotmpcs_ros/rviz/boxer_example.rviz new file mode 100644 index 0000000..e85802c --- /dev/null +++ b/ros_bridge/src/robotmpcs_ros/rviz/boxer_example.rviz @@ -0,0 +1,496 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /LaserScan1 + - /TF1 + - /TF1/Frames1 + Splitter Ratio: 0.5 + Tree Height: 746 + - 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: LaserScan +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 + - 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: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + 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: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /front/scan + Unreliable: false + 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 + 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: /mpc/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 46.13591384887695 + 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: 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.5653980374336243 + Target Frame: + Yaw: 1.59039306640625 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1043 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000015600000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000006240000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + 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 8e1fbf7..d92559b 100755 --- a/ros_bridge/src/robotmpcs_ros/src/mpc_planner_node +++ b/ros_bridge/src/robotmpcs_ros/src/mpc_planner_node @@ -1,15 +1,25 @@ #!/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, LaserScan import rospkg +import sys import rospy -from geometry_msgs.msg import Pose, Twist +import tf +import tf2_ros +import math +import sensor_msgs.point_cloud2 as pc2 +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 +from robotmpcs.utils.free_space_decomposition import FreeSpaceDecomposition def get_rotation(pose: Pose) -> float: orientation_q = pose.orientation @@ -30,6 +40,7 @@ class MPCNode(): def __init__(self): rospy.init_node("mpc_node") + self._transform = None self.establish_ros_connections() self._dt = rospy.get_param('/mpc/time_step') self._rate = rospy.Rate(1/self._dt) @@ -37,9 +48,17 @@ class MPCNode(): self.init_planner() self.set_mpc_parameter() self.init_arrays() + self.init_visuals() + self.tf_buffer = tf2_ros.Buffer() + self.listener = tf.TransformListener() + + + + 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}, @@ -56,7 +75,26 @@ 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) + + 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) @@ -69,6 +107,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,27 +118,75 @@ 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( - "/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", Float64MultiArray, self._goal_cb + "/mpc/goal", PoseStamped, self._goal_cb, queue_size=1, + ) + + self._lidar_sub = rospy.Subscriber( + "/front/scan", LaserScan, self._lidar_cb, queue_size=1, ) - 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 = { @@ -113,7 +202,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 +217,67 @@ class MPCNode(): odom_msg.twist.twist.angular.z, ]) + def _lidar_cb(self, msg: PointCloud2): + cloud_points = [] + angle = msg.angle_min + + 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) + + cloud_points.append([x, y, z]) + angle += msg.angle_increment + + 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)) + + + if self._translation is not None: + cloud_points = np.array(cloud_points) + + + R = quaternion_to_rotation_matrix(self._rotation) + + 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 + + + self._lidar_pointcloud = np.array(transformed_points) + + + 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 = self._translation[:2] + + lidar_position = np.array([position_lidar[0], position_lidar[1], 0.0]) + + + self._fsd.set_position(lidar_position) + if len(point_cloud)>0: + 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() @@ -137,17 +287,79 @@ class MPCNode(): self._qudot = vel_action def run(self): + + while not rospy.is_shutdown(): + linear_constraints = [] + if self._lidar_pointcloud is not None: + 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] + + 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) + linear_constraints.append(linear_constraints_j) + halfplanes.append(halfplanes_j) + + self._planner.setLinearConstraints(linear_constraints, r_body=self._r_body) + print('linear constraints are set') + if self._goal: - self._action, _ = self._planner.computeAction(self._q, self._qdot, self._qudot) - #self._action = np.array([1.0, 0.2]) - rospy.loginfo(self._action) + self._action, self._output, self._exitflag = self._planner.computeAction(self._q, self._qdot, self._qudot) + rospy.loginfo(self._action) + rospy.loginfo(self._exitflag) + print(self._output) + + 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: + self.visual_constraints.add_constraint_line(constraint[0][:2], -constraint[0][-1], length=10.0) + + + if self._goal is not None: + goal_pose = Pose() + 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): + 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() + mpc_node._output: Union[dict, None] = None mpc_node.run() except rospy.ROSInterruptException: pass 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 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