diff --git a/README.md b/README.md index 229b5cf..344dddb 100644 --- a/README.md +++ b/README.md @@ -46,6 +46,7 @@ Before installing the robot software, you need to install the simulator : https: git clone https://github.com/clubcapra/rove.git cd rove vcs import src < rove.repos +echo "export GZ_VERSION=harmonic" >> ~/.bashrc && source ~/.bashrc sudo rosdep init rosdep update rosdep install --from-paths src --ignore-src -r -y diff --git a/rove.repos b/rove.repos index 3919312..9c7cebe 100644 --- a/rove.repos +++ b/rove.repos @@ -6,4 +6,8 @@ repositories: ovis_ros2: type: git url: https://github.com/clubcapra/ovis_ros2 - version: new-env-with-arm \ No newline at end of file + version: fix-gazebo-joy + gz_ros2_control: + type: git + url: https://github.com/clubcapra/gz_ros2_control.git + version: iron \ No newline at end of file diff --git a/src/rove_bringup/launch/sim.launch.py b/src/rove_bringup/launch/sim.launch.py index 3db90ea..aff4ee6 100644 --- a/src/rove_bringup/launch/sim.launch.py +++ b/src/rove_bringup/launch/sim.launch.py @@ -3,109 +3,110 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import IncludeLaunchDescription - from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import Command from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue -from math import pi def generate_launch_description(): # Get the launch directory - pkg_rove_bringup = get_package_share_directory('rove_bringup') - pkg_rove_description = get_package_share_directory('rove_description') - pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') - - # Get the URDF file - urdf_path = os.path.join(pkg_rove_description, 'urdf', 'rove.urdf.xacro') - robot_desc = ParameterValue(Command(['xacro ', urdf_path]), value_type=str) + pkg_rove_bringup = get_package_share_directory("rove_bringup") + pkg_rove_description = get_package_share_directory("rove_description") + pkg_ros_gz_sim = get_package_share_directory("ros_gz_sim") + pkg_ovis = get_package_share_directory("ovis_bringup") # Get simulation file - world_file_name = 'worlds/base_world.world' + world_file_name = "worlds/base_world.world" world = os.path.join(pkg_rove_description, world_file_name) # Setup to launch the simulator and Gazebo world gz_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')), - launch_arguments={'gz_args': "-v 4 -r " + world}.items(), + os.path.join(pkg_ros_gz_sim, "launch", "gz_sim.launch.py") + ), + launch_arguments={"gz_args": "-v 4 -r " + world}.items(), ) - walls_file_path = os.path.join(pkg_rove_description, 'worlds', 'walls.sdf') + walls_file_path = os.path.join(pkg_rove_description, "worlds", "walls.sdf") spawn_walls = Node( - package='ros_gz_sim', - executable='create', - arguments=['-file', walls_file_path, - '-name', 'walls', - '-x', '0', - '-y', '0', - '-z', '0'], - output='screen', + package="ros_gz_sim", + executable="create", + arguments=[ + "-file", + walls_file_path, + "-name", + "walls", + "-x", + "0", + "-y", + "0", + "-z", + "0", + ], + output="screen", ) - actor_file_path = os.path.join(pkg_rove_description, 'worlds', 'actor.sdf') + actor_file_path = os.path.join(pkg_rove_description, "worlds", "actor.sdf") spawn_actor = Node( - package='ros_gz_sim', - executable='create', - arguments=['-file', actor_file_path, - '-name', 'actor', - '-topic', 'actor_pose', - '-x', '0', - '-y', '0', - '-z', '0.1'], - output='screen', + package="ros_gz_sim", + executable="create", + arguments=[ + "-file", + actor_file_path, + "-name", + "actor", + "-x", + "0", + "-y", + "0", + "-z", + "0.06", + ], + output="screen", ) - yaw = -pi / 2 - # Spawn robot spawn_rove = Node( - package='ros_gz_sim', - executable='create', + package="ros_gz_sim", + executable="create", arguments=[ - '-name', 'rove', - '-topic', 'robot_description', - '-x', '0', - '-y', '0', - '-z', '0.1', - '-Y', str(yaw), + "-name", + "rove", + "-topic", + "robot_description", + "-x", + "0", + "-y", + "0", + "-z", + "0.1", ], - output='screen', - ) - - # Takes the description and joint angles as inputs and publishes - # the 3D poses of the robot links - robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - output='both', - parameters=[ - {'robot_description': robot_desc}, - {"use_sim_time": True, } - ] + output="screen", ) # fake human tracker human_tracker = Node( - package='rove_navigation', - executable='green_person_tracker', - name='green_person_tracker', - output='screen', + package="rove_navigation", + executable="green_person_tracker", + name="green_person_tracker", + output="screen", ) # Bridge ROS topics and Gazebo messages for establishing communication bridge = Node( - package='ros_gz_bridge', - executable='parameter_bridge', - parameters=[{ - 'config_file': os.path.join(pkg_rove_description, 'config', - 'default_bridge.yaml'), - 'qos_overrides./tf_static.publisher.durability': 'transient_local', - "use_sim_time": True, - }], - output='screen' + package="ros_gz_bridge", + executable="parameter_bridge", + parameters=[ + { + "config_file": os.path.join( + pkg_rove_description, "config", "default_bridge.yaml" + ), + "qos_overrides./tf_static.publisher.durability": "transient_local", + "use_sim_time": True, + } + ], + output="screen", ) common = IncludeLaunchDescription( @@ -117,13 +118,29 @@ def generate_launch_description(): }.items(), ) - return LaunchDescription([ + + ovis = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_ovis, "launch", "sim.launch.py"), + ), + launch_arguments={ + "with_rove": "true", + "with_joy": "false", + "ovis_base_origin": "0.22 0 0.34 0 0 3.14", + }.items(), + ) + + + + return LaunchDescription( + [ gz_sim, bridge, - robot_state_publisher, spawn_walls, spawn_actor, spawn_rove, common, - human_tracker, - ]) + # human_tracker, + ovis, + ] + ) diff --git a/src/rove_bringup/src/rove_controller_node.cpp b/src/rove_bringup/src/rove_controller_node.cpp index 1857cea..7de7c19 100644 --- a/src/rove_bringup/src/rove_controller_node.cpp +++ b/src/rove_bringup/src/rove_controller_node.cpp @@ -10,184 +10,198 @@ using GripperCommand = control_msgs::action::GripperCommand; using GoalHandleGripperCommand = rclcpp_action::ClientGoalHandle; -enum ControllerMode{ - FLIPPER_MODE = 0, - ARM_MODE = 1, +enum ControllerMode +{ + TRACKS_MODE = 0, + ARM_MODE = 1, + FLIPPER_MODE = 2, }; - -class RoveController : public rclcpp::Node { +class RoveController : public rclcpp::Node +{ public: - RoveController() : Node("rove_controller_node") { - // Buttons - A = declare_parameter("A", 0); - B = declare_parameter("B", 1); - X = declare_parameter("X", 2); - Y = declare_parameter("Y", 3); - LB = declare_parameter("LB", 4); - RB = declare_parameter("RB", 5); - VIEW = declare_parameter("VIEW", 6); - MENU = declare_parameter("MENU", 7); - XBOX = declare_parameter("XBOX", 8); - LS = declare_parameter("LS", 9); - RS = declare_parameter("RS", 10); - SHARE = declare_parameter("SHARE", 11); - - // Axes - LS_X = declare_parameter("LS_X", 0); - LS_Y = declare_parameter("LS_Y", 1); - LT = declare_parameter("LT", 2); - RS_X = declare_parameter("RS_X", 3); - RS_Y = declare_parameter("RS_Y", 4); - RT = declare_parameter("RT", 5); - D_PAD_X = declare_parameter("D_PAD_X", 6); - D_PAD_Y = declare_parameter("D_PAD_Y", 7); - - // Subscribe to the joy topic - joy_sub_ = create_subscription( - "/joy", 10, std::bind(&RoveController::joyCallback, this, std::placeholders::_1) - ); - // cmd_vel_sub_ = create_subscription("/cmd_vel", 1, std::bind(&RoveController::cmdvelCallback, this, std::placeholders::_1)); - - joy_pub_ = create_publisher("/rove/joy", 1); - - // auto x = create_wall_timer( - // std::chrono::milliseconds(1000/20), std::bind(&RoveController::cmdvelTimerCallback, this, std::placeholders::_1) - // ); - - previous_msg_ = sensor_msgs::msg::Joy(); - previous_msg_.axes.resize(6, 0.0); - previous_msg_.buttons.resize(10, 0); - - teleop_msg_ = sensor_msgs::msg::Joy(); - teleop_msg_.axes.resize(2,0); - teleop_msg_.buttons.resize(1,0); - - gripper_action_client_ = rclcpp_action::create_client(this, "/robotiq_gripper_controller/gripper_cmd"); - - // Check if action server is available - if (!gripper_action_client_->wait_for_action_server(std::chrono::seconds(10))) { - RCLCPP_ERROR(get_logger(), "Action server not available after waiting"); - } - - // cmd_vel_msg_ = geometry_msgs::msg::Twist(); - // cmd_vel_stamped_msg_ = geometry_msgs::msg::TwistStamped(); + RoveController() : Node("rove_controller_node") + { + // Buttons + A = declare_parameter("A", 0); + B = declare_parameter("B", 1); + X = declare_parameter("X", 2); + Y = declare_parameter("Y", 3); + LB = declare_parameter("LB", 4); + RB = declare_parameter("RB", 5); + VIEW = declare_parameter("VIEW", 6); + MENU = declare_parameter("MENU", 7); + XBOX = declare_parameter("XBOX", 8); + LS = declare_parameter("LS", 9); + RS = declare_parameter("RS", 10); + SHARE = declare_parameter("SHARE", 11); + + // Axes + LS_X = declare_parameter("LS_X", 0); + LS_Y = declare_parameter("LS_Y", 1); + LT = declare_parameter("LT", 2); + RS_X = declare_parameter("RS_X", 3); + RS_Y = declare_parameter("RS_Y", 4); + RT = declare_parameter("RT", 5); + D_PAD_X = declare_parameter("D_PAD_X", 6); + D_PAD_Y = declare_parameter("D_PAD_Y", 7); + + // Subscribe to the joy topic + joy_sub_ = create_subscription( + "/joy", 10, std::bind(&RoveController::joyCallback, this, std::placeholders::_1)); + + joy_pub_rove_ = create_publisher("/rove/joy", 1); + joy_pub_ovis_ = create_publisher("/ovis/joy", 1); + + previous_msg_ = sensor_msgs::msg::Joy(); + previous_msg_.axes.resize(6, 0.0); + previous_msg_.buttons.resize(10, 0); + + teleop_msg_ = sensor_msgs::msg::Joy(); + teleop_msg_.axes.resize(2, 0); + teleop_msg_.buttons.resize(1, 0); + + gripper_action_client_ = + rclcpp_action::create_client(this, "/robotiq_gripper_controller/gripper_cmd"); + + if (!gripper_action_client_->wait_for_action_server(std::chrono::seconds(10))) + { + RCLCPP_ERROR(get_logger(), "Action server not available after waiting"); } + } private: - int A, B, X, Y, LB, RB, VIEW, MENU, XBOX, LS, RS, SHARE, LS_X, LS_Y, LT, RS_X, RS_Y, RT, D_PAD_X, D_PAD_Y; - - int selected_mode = FLIPPER_MODE; - - rclcpp_action::Client::SharedPtr gripper_action_client_; - - void nextMode(){ - selected_mode = (selected_mode + 1) % 2; // Number of modes - RCLCPP_INFO(get_logger(), "profil %i", selected_mode); - } - - void common_action(const sensor_msgs::msg::Joy::SharedPtr joy_msg) { - if (buttton_down(joy_msg, Y)){ - nextMode(); - } - - if (buttton_down(joy_msg, B)){ - gripper_opened_ = !gripper_opened_; - // position, effort - if(gripper_opened_){ - sendGripperGoal(0.0, 10.0); - } - else{ - sendGripperGoal(1.0, 10.0); - } - } - - if(buttton_up(joy_msg, B)){ - log("Common B up"); - } - } - - void arm_action(const sensor_msgs::msg::Joy::SharedPtr joy_msg) { - log("Arm"); - if(button_pressed(joy_msg, B)){ - log("Arm B pressed"); - } - - if(buttton_up(joy_msg, X)){ - log("Arm X up"); - } - - if(buttton_down(joy_msg, X)){ - log("Arm X down"); - } - - } - - void flipper_action(const sensor_msgs::msg::Joy::SharedPtr joy_msg) { - log("Flipper..."); - teleop_msg_.axes[0] = joy_msg->axes[LS_X]; - teleop_msg_.axes[1] = joy_msg->axes[LS_Y]; - teleop_msg_.buttons[0] = joy_msg->buttons[A]; - joy_pub_->publish(teleop_msg_); - } - - bool buttton_down(sensor_msgs::msg::Joy::SharedPtr curr_msg, int index){ - return previous_msg_.buttons[index] != curr_msg->buttons[index] && curr_msg->buttons[index] == 1; + int A, B, X, Y, LB, RB, VIEW, MENU, XBOX, LS, RS, SHARE; + int LS_X, LS_Y, LT, RS_X, RS_Y, RT, D_PAD_X, D_PAD_Y; + + int selected_mode = TRACKS_MODE; + bool publish_to_rove = true; // Toggle between rove and ovis + + rclcpp_action::Client::SharedPtr gripper_action_client_; + bool gripper_opened_ = true; + + void nextMode() + { + selected_mode = (selected_mode + 1) % 3; // Now cycles through 3 modes + + // Reset previous message state when switching modes + previous_msg_ = sensor_msgs::msg::Joy(); + previous_msg_.axes.resize(6, 0.0); + previous_msg_.buttons.resize(10, 0); + + std::string mode_name; + switch (selected_mode) + { + case TRACKS_MODE: + mode_name = "TRACKS"; + break; + case ARM_MODE: + mode_name = "ARM"; + break; + case FLIPPER_MODE: + mode_name = "FLIPPER"; + break; } - bool buttton_up(sensor_msgs::msg::Joy::SharedPtr curr_msg, int index){ - return previous_msg_.buttons[index] != curr_msg->buttons[index] && curr_msg->buttons[index] == 0; - } - bool button_pressed(sensor_msgs::msg::Joy::SharedPtr curr_msg, int index){ - return previous_msg_.buttons[index] == curr_msg->buttons[index] && curr_msg->buttons[index] == 1; + RCLCPP_INFO(get_logger(), "Current mode: %s", mode_name.c_str()); + } + + void common_controls(const sensor_msgs::msg::Joy::SharedPtr joy_msg) + { + // Mode switching with MENU button + if (button_down(joy_msg, MENU)) + { + nextMode(); } - void log(const char *text){ - static rclcpp::Clock clk{}; - static const char* last = nullptr; - if (last == text) return; - last = text; - // RCLCPP_INFO_THROTTLE(get_logger(), clk, 2000, text); - RCLCPP_INFO(get_logger(), text); + // Gripper control with SHARE button + if (button_down(joy_msg, SHARE)) + { + gripper_opened_ = !gripper_opened_; + sendGripperGoal(gripper_opened_ ? 0.0 : 1.0, 10.0); } - - void joyCallback(const sensor_msgs::msg::Joy::SharedPtr joy_msg) { - common_action(joy_msg); - switch (selected_mode){ - case ARM_MODE: - arm_action(joy_msg); - break; - case FLIPPER_MODE: - flipper_action(joy_msg); - break; - } - - previous_msg_ = *joy_msg; + } + + void tracks_mode(const sensor_msgs::msg::Joy::SharedPtr joy_msg) + { + teleop_msg_.axes[0] = joy_msg->axes[LS_X]; + teleop_msg_.axes[1] = joy_msg->axes[LS_Y]; + teleop_msg_.buttons[0] = joy_msg->buttons[A]; + joy_pub_rove_->publish(teleop_msg_); + } + + void arm_mode(const sensor_msgs::msg::Joy::SharedPtr joy_msg) + { + // Create a modified joy message with inverted axes + sensor_msgs::msg::Joy modified_joy = *joy_msg; + + // Invert the relevant axes (Y axes are typically the ones that need inversion) + modified_joy.axes[LS_Y] = -joy_msg->axes[LS_Y]; + modified_joy.axes[RS_Y] = -joy_msg->axes[RS_Y]; + + joy_pub_ovis_->publish(modified_joy); + } + + void flipper_mode(const sensor_msgs::msg::Joy::SharedPtr joy_msg) + { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, "Flipper mode not implemented yet"); + } + + bool button_down(const sensor_msgs::msg::Joy::SharedPtr curr_msg, int index) + { + return previous_msg_.buttons[index] != curr_msg->buttons[index] && curr_msg->buttons[index] == 1; + } + + bool button_up(const sensor_msgs::msg::Joy::SharedPtr curr_msg, int index) + { + return previous_msg_.buttons[index] != curr_msg->buttons[index] && curr_msg->buttons[index] == 0; + } + + bool button_pressed(const sensor_msgs::msg::Joy::SharedPtr curr_msg, int index) + { + return curr_msg->buttons[index] == 1; + } + + void sendGripperGoal(double position, double max_effort) + { + auto goal_msg = GripperCommand::Goal(); + goal_msg.command.position = position; + goal_msg.command.max_effort = max_effort; + RCLCPP_INFO(get_logger(), "Sending gripper command: pos=%.2f, effort=%.2f", position, max_effort); + gripper_action_client_->async_send_goal(goal_msg); + } + + void joyCallback(const sensor_msgs::msg::Joy::SharedPtr joy_msg) + { + common_controls(joy_msg); + + switch (selected_mode) + { + case TRACKS_MODE: + tracks_mode(joy_msg); + break; + case ARM_MODE: + arm_mode(joy_msg); + break; + case FLIPPER_MODE: + flipper_mode(joy_msg); + break; } - void sendGripperGoal(double position, double max_effort) { - auto goal_msg = GripperCommand::Goal(); - goal_msg.command.position = position; - goal_msg.command.max_effort = max_effort; - - log("Sending goal"); - - gripper_action_client_->async_send_goal(goal_msg); - } + previous_msg_ = *joy_msg; + } - rclcpp::Subscription::SharedPtr joy_sub_; - // rclcpp::Subscription::SharedPtr cmd_vel_sub_; - rclcpp::Publisher::SharedPtr joy_pub_; - sensor_msgs::msg::Joy previous_msg_; - sensor_msgs::msg::Joy teleop_msg_; - // geometry_msgs::msg::Twist cmd_vel_msg_; - // geometry_msgs::msg::TwistStamped cmd_vel_stamped_msg_; - bool gripper_opened_ = true; + rclcpp::Subscription::SharedPtr joy_sub_; + rclcpp::Publisher::SharedPtr joy_pub_rove_; + rclcpp::Publisher::SharedPtr joy_pub_ovis_; + sensor_msgs::msg::Joy previous_msg_; + sensor_msgs::msg::Joy teleop_msg_; }; -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; } diff --git a/src/rove_description/config/basic.rviz b/src/rove_description/config/basic.rviz index 881b1b1..ef0b487 100644 --- a/src/rove_description/config/basic.rviz +++ b/src/rove_description/config/basic.rviz @@ -5,6 +5,7 @@ Panels: Property Tree Widget: Expanded: - /DepthCloud1/Auto Size1 + - /RobotModel2/Status1 Splitter Ratio: 0.5 Tree Height: 325 - Class: rviz_common/Selection @@ -472,6 +473,79 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /ovis/robot_description + 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_link: + Alpha: 1 + Show Axes: false + Show Trail: false + ovis_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + ovis_end_effector: + Alpha: 1 + Show Axes: false + Show Trail: false + ovis_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ovis_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ovis_link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ovis_link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ovis_link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ovis_link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ovis_link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -518,25 +592,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 36.84917449951172 + Distance: 5.473262786865234 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.4458295702934265 - Y: 3.5056796073913574 - Z: -1.5910545587539673 + X: 2.4571151733398438 + Y: -1.7171390056610107 + Z: -1.659576654434204 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.914797306060791 + Pitch: 0.8547973036766052 Target Frame: Value: Orbit (rviz) - Yaw: 4.8786468505859375 + Yaw: 3.2836241722106934 Saved: ~ Window Geometry: Camera: diff --git a/src/rove_description/config/default_bridge.yaml b/src/rove_description/config/default_bridge.yaml index 9ab1e6e..4ac39c3 100644 --- a/src/rove_description/config/default_bridge.yaml +++ b/src/rove_description/config/default_bridge.yaml @@ -73,4 +73,18 @@ gz_topic_name: "/camera_info" ros_type_name: "sensor_msgs/msg/CameraInfo" gz_type_name: "gz.msgs.CameraInfo" - direction: GZ_TO_ROS \ No newline at end of file + direction: GZ_TO_ROS + +# Bridge for dynamic transforms +- ros_topic_name: "/tf" + gz_topic_name: "/tf" + ros_type_name: "tf2_msgs/msg/TFMessage" + gz_type_name: "gz.msgs.Pose_V" + direction: BIDIRECTIONAL + +# Bridge for static transforms +- ros_topic_name: "/tf_static" + gz_topic_name: "/tf_static" + ros_type_name: "tf2_msgs/msg/TFMessage" + gz_type_name: "gz.msgs.Pose_V" + direction: BIDIRECTIONAL diff --git a/src/rove_description/urdf/gazebo.urdf.xacro b/src/rove_description/urdf/gazebo.urdf.xacro index 161e87b..38ca36e 100644 --- a/src/rove_description/urdf/gazebo.urdf.xacro +++ b/src/rove_description/urdf/gazebo.urdf.xacro @@ -41,5 +41,13 @@ + + + Gazebo/Grey + 100.0 + 100.0 + 1000000.0 + 1.0 + diff --git a/src/rove_description/urdf/rove.urdf.xacro b/src/rove_description/urdf/rove.urdf.xacro index dec91b8..71da79b 100644 --- a/src/rove_description/urdf/rove.urdf.xacro +++ b/src/rove_description/urdf/rove.urdf.xacro @@ -1,63 +1,145 @@ - + - + - + - + - + - + - + - + - + - + - + - + - + - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + - + + --> - + \ No newline at end of file diff --git a/src/rove_description/urdf/sensor.urdf.xacro b/src/rove_description/urdf/sensor.urdf.xacro index b250f45..915b6de 100644 --- a/src/rove_description/urdf/sensor.urdf.xacro +++ b/src/rove_description/urdf/sensor.urdf.xacro @@ -5,7 +5,7 @@ - + @@ -27,13 +27,13 @@ - + - + @@ -45,19 +45,18 @@ - + - + - @@ -67,20 +66,20 @@ - + - + - + - + Gazebo/Black diff --git a/src/rove_navigation/rove_navigation/model_attacher.py b/src/rove_navigation/rove_navigation/model_attacher.py new file mode 100755 index 0000000..5b293ba --- /dev/null +++ b/src/rove_navigation/rove_navigation/model_attacher.py @@ -0,0 +1,64 @@ +import rclpy +from rclpy.node import Node +from ros_gz_interfaces.srv import SpawnEntity + +class ModelAttacherNode(Node): + def __init__(self): + super().__init__('model_attacher_node') + + self.declare_parameter('parent_model', 'rove') + self.declare_parameter('child_model', 'ovis') + self.declare_parameter('parent_link', 'base_link') + self.declare_parameter('child_link', 'base_link') + + self.parent_model = self.get_parameter('parent_model').get_parameter_value().string_value + self.child_model = self.get_parameter('child_model').get_parameter_value().string_value + self.parent_link = self.get_parameter('parent_link').get_parameter_value().string_value + self.child_link = self.get_parameter('child_link').get_parameter_value().string_value + + # Create service client + self.spawn_entity_client = self.create_client( + SpawnEntity, + '/world/default/create' + ) + + while not self.spawn_entity_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('Waiting for SpawnEntity service...') + + self.attach_models() + + def attach_models(self): + # Create an SDF string for a fixed joint + joint_sdf = f""" + + + + + {self.parent_model}::{self.parent_link} + {self.child_model}::{self.child_link} + + + + """ + + request = SpawnEntity.Request() + request.xml = joint_sdf + request.name = "joint_attachment" + + future = self.spawn_entity_client.call_async(request) + rclpy.spin_until_future_complete(self, future) + + if future.result() is not None: + self.get_logger().info('Successfully created joint between models') + else: + self.get_logger().error('Failed to create joint between models') + +def main(args=None): + rclpy.init(args=args) + node = ModelAttacherNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/rove_navigation/rove_navigation/ovis_pull.py b/src/rove_navigation/rove_navigation/ovis_pull.py new file mode 100755 index 0000000..21f2142 --- /dev/null +++ b/src/rove_navigation/rove_navigation/ovis_pull.py @@ -0,0 +1,71 @@ +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Pose +from ros_gz_interfaces.srv import SetEntityPose, SpawnEntity +import tf2_ros + +class PositionLockNode(Node): + def __init__(self): + super().__init__('position_lock_node') + + self.declare_parameter('rove_name', 'rove') + self.declare_parameter('ovis_name', 'ovis') + self.declare_parameter('update_rate', 10.0) # Hz + self.declare_parameter('offset_x', 0.25) + self.declare_parameter('offset_y', 0.0) + self.declare_parameter('offset_z', 0.25) + + self.rove_name = self.get_parameter('rove_name').get_parameter_value().string_value + self.ovis_name = self.get_parameter('ovis_name').get_parameter_value().string_value + self.update_rate = self.get_parameter('update_rate').get_parameter_value().double_value + self.offset_x = self.get_parameter('offset_x').get_parameter_value().double_value + self.offset_y = self.get_parameter('offset_y').get_parameter_value().double_value + self.offset_z = self.get_parameter('offset_z').get_parameter_value().double_value + + # Set up the TF listener + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + + # Set up the SetEntityPose service client + self.set_entity_pose_client = self.create_client(SetEntityPose, '/world/default/set_pose') + while not self.set_entity_pose_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('Waiting for SetEntityPose service...') + + # Timer to periodically update the position + self.timer = self.create_timer(1.0 / self.update_rate, self.update_position) + + def update_position(self): + try: + # Look up the latest transform for `rove` + transform = self.tf_buffer.lookup_transform('world', 'base_link', rclpy.time.Time()) + # Calculate the target pose for `ovis` with the given offsets + target_pose = Pose() + target_pose.position.x = transform.transform.translation.x + self.offset_x + target_pose.position.y = transform.transform.translation.y + self.offset_y + target_pose.position.z = transform.transform.translation.z + self.offset_z + + # Use the quaternion orientation from `rove` directly + target_pose.orientation = transform.transform.rotation + + # Set the new pose of `ovis` + set_pose_request = SetEntityPose.Request() + set_pose_request.name = self.ovis_name + set_pose_request.pose = target_pose + future = self.set_entity_pose_client.call_async(set_pose_request) + rclpy.spin_until_future_complete(self, future) + if future.result() is None or not future.result().success: + self.get_logger().error(f'Failed to set pose of {self.ovis_name}') + else: + self.get_logger().info(f'Successfully updated pose of {self.ovis_name}') + except Exception as e: + self.get_logger().error(f'Error retrieving transform: {e}') + +def main(args=None): + rclpy.init(args=args) + node = PositionLockNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/rove_navigation/setup.py b/src/rove_navigation/setup.py index 9801881..f3d2c1a 100644 --- a/src/rove_navigation/setup.py +++ b/src/rove_navigation/setup.py @@ -32,6 +32,8 @@ 'exploration = rove_navigation.exploration:main', 'lost_connection = rove_navigation.lost_connection:main', 'mule_behavior = rove_navigation.mule_behavior:main', + 'ovis_pull = rove_navigation.ovis_pull:main', + 'model_attacher = rove_navigation.model_attacher:main', ], }, )