0
$\begingroup$

I am trying to spawn an object in gazebo at certain height and apply force to it in x direction at the same time(when the object is in the air). The object is being spawned successfully. But the force applied is not observable visually although I am getting Initial force applied successfully in the terminal. I am sure adequate amount of force is being applied as I applied the same amount of force to the object directly through GUI in gazebo and the object moves. I am using /apply_link_wrench service to do this. I am unable to find /gazebo/apply_body_wrench service when running ros2 service list.

Spawning script

spawn.py:

import rclpy
from rclpy.node import Node
from gazebo_msgs.srv import SpawnEntity, ApplyLinkWrench, DeleteEntity
from geometry_msgs.msg import Point, Quaternion, Wrench
import xacro

class BallSpawner(Node):

    def __init__(self):
        super().__init__('ball_spawner')
            
        # Initializing Clients    
        self.spawn_client = self.create_client(SpawnEntity, '/spawn_entity')
        self.apply_wrench_client = self.create_client(ApplyLinkWrench, '/apply_link_wrench')
        
        
        # Function Calls
        self.spawn_ball()
        self.apply_force()
        
        
    def spawn_ball(self):
        while not self.spawn_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('Service not available, waiting again...')
        
        request = SpawnEntity.Request()
        
        request.name = "my_object"
        
        request.xml = xacro.process_file('/home/ball_ws/src/ball_spawner/description/temp.urdf').toxml()
        
        
        request.robot_namespace = "/"
        request.initial_pose.position = Point(x=0.0, y=0.0, z=1.0)
        request.initial_pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)        
        
        self.future = self.spawn_client.call_async(request)
        rclpy.spin_until_future_complete(self, self.future)
        if self.future.result() is not None:
            self.get_logger().info('Object spawned successfully')
        else:
            self.get_logger().error('Failed to spawn object')
            
    def apply_force(self):
        while not self.apply_wrench_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('Service not available, waiting again...') 
            
        request = ApplyLinkWrench.Request()
        request.link_name = "my_object::base_link"
        request.reference_frame = "world"
        request.wrench.force.x = 10000.0  # Apply force in x-direction
        request.wrench.force.y = 0.0
        request.wrench.force.z = 0.0
        request.duration.sec = 10  # Apply force for 1 second
        
        self.future = self.apply_wrench_client.call_async(request)
        rclpy.spin_until_future_complete(self, self.future)
        if self.future.result() is not None:
            self.get_logger().info('Initial force applied successfully')
        else:
            self.get_logger().error('Failed to apply initial force')
            
                   
            
def main(args=None):
    rclpy.init(args=args)
    spawner = BallSpawner()
    rclpy.spin_once(spawner)
    spawner.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

URDF file

temp.urdf

<robot name="my_object">
  <link name="base_link">
    <inertial>
      <mass value="1.0"/>
      <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
    </inertial>
    <collision>
      <geometry>
        <box size="0.5 0.5 0.5"/>
      </geometry>
    </collision>
    <visual>
      <geometry>
        <box size="0.5 0.5 0.5"/>
      </geometry>
    </visual>
  </link>
</robot>

Launch File

rsp.launch.py

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node

from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

import xacro


def generate_launch_description():
    
    
    # Process the URDF file
    pkg_path = os.path.join(get_package_share_directory('ball_spawner'))
    xacro_file = os.path.join(pkg_path,'description','ball.urdf.xacro')
    robot_description_config = xacro.process_file(xacro_file)
    
    # ROBOT STATE PUBLISHER    
    # Create a robot_state_publisher node
    params = {'robot_description': robot_description_config.toxml()}
    node_robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        output='screen',
        parameters=[params]
    )


    # RVIZ
    rviz = Node(
            package='rviz2',
            executable='rviz2',
            output='screen'
        )
    
    # Include the Gazebo launch file, provided by the gazebo_ros package
    gazebo = IncludeLaunchDescription(
                PythonLaunchDescriptionSource([os.path.join(
                    get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]),
                    launch_arguments={'world': "./src/ball_spawner/worlds/world_1.world"}.items()
             )

    # Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot.
    spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
                        arguments=['-topic', 'robot_description',
                                   '-entity', 'ball_bot'],
                        output='screen')
    

    # Launch!
    return LaunchDescription([
        node_robot_state_publisher,
        rviz,
        gazebo,
        #spawn_entity,
    ])
```
$\endgroup$

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.