Implementation of Team 0's Obstacle Detection #3
Conversation
Custom algorithm
Feat/ros2 node
…uc and implemented ROS. Processing from ROS topics data functionning but not publishing
| x_r = ranges * np.cos(angles) # (N,) | ||
| y_r = ranges * np.sin(angles) # (N,) | ||
| robot_coords = np.stack((x_r, y_r), axis=1) # (N,2) |
There was a problem hiding this comment.
C'est les coords des points du scan Lidar, pas les robot_coords. Mais tu as raison que c'est par rapport au référentiel du robot. Je changerais le nom pour scan_points ou scan_points_in_robot_frame
| rotation = np.array([[cos_yaw, -sin_yaw], [sin_yaw, cos_yaw]]) # (2,2) | ||
| reference_coords = robot_coords @ rotation.T # (N,2) | ||
| reference_coords += np.array([pose.position.x, pose.position.y]) |
There was a problem hiding this comment.
Ici je rendrais explicite la transformation. On veut passer du frame du robot au frame de la map. J'utiliserais aussi une transformation homogène pour combiner la rotation et la translation dans une seule matrice 4x4.
| # Destroy the node explicitly | ||
| # (optional - otherwise it will be done automatically | ||
| # when the garbage collector destroys the node object) |
There was a problem hiding this comment.
| # Destroy the node explicitly | |
| # (optional - otherwise it will be done automatically | |
| # when the garbage collector destroys the node object) |
| void publish_obstacle_points(const std::vector<cv::Point2d>& points, const rclcpp::Time& stamp) { | ||
| geometry_msgs::msg::PoseArray pose_array; | ||
| pose_array.header.stamp = stamp; | ||
| pose_array.header.frame_id = map_->header.frame_id; | ||
| pose_array.poses.resize(points.size()); | ||
| for (size_t i = 0; i < points.size(); ++i) { | ||
| pose_array.poses[i].position.x = points[i].x; | ||
| pose_array.poses[i].position.y = points[i].y; | ||
| } | ||
| obstacles_pub_->publish(pose_array); | ||
| } |
There was a problem hiding this comment.
Pour pouvoir être utilisé dans d'autres algos qui vont dynamiquement changer le comportement du robot, il va falloir implémenter ce publisher de pose_array dans le noeud python
4 participants
Added obstacle detection with ROS2