Python调用‘/gazebo/set_link_state‘服务

Housebigger

已于 2024-05-03 17:50:13 修改

阅读量816

点赞数 9

分类专栏： ROS-gazebo 文章标签： python

于 2024-05-03 17:36:12 首次发布

本文链接：https://blog.csdn.net/qq_41960915/article/details/138419673

版权

ROS-gazebo 专栏收录该内容

3 篇文章 0 订阅

订阅专栏

背景：

在ros-melodic-gazebo9仿真环境里有一个运动中的机器人，为了使它在base_link运动过程中其末端link仍然保持在当前世界位置并相对世界坐标静止，我用了下述Python代码调用get_link_state和set_link_state，首先获取当前的位姿，然后循环执行set link state的服务。

以下是第一版有故障的脚本：

#!/usr/bin/env python

import rospy
from gazebo_msgs.srv import GetLinkState, GetLinkStateRequest, SetLinkState, SetLinkStateRequest
from geometry_msgs.msg import Pose, Point, Quaternion
from std_msgs.msg import Header

def get_current_pose(link_name):
    """
    Get the current pose of the specified link.
    
    :param link_name: The name of the link to get the pose for
    :return: The current pose of the link as a Pose object
    """
    rospy.wait_for_service('/gazebo/get_link_state')
    try:
        get_link_state_service = rospy.ServiceProxy('/gazebo/get_link_state', GetLinkState)
        get_link_state_request = GetLinkStateRequest()
        get_link_state_request.link_name = link_name
        get_link_state_request.reference_frame = 'world'
        response = get_link_state_service(get_link_state_request)
        return response.pose
    except rospy.ServiceException as e:
        print("Failed to call get link state service: %s"%e)

def set_link_state(link_name, pose):
    """
    Set the state of the specified link to keep it static relative to the world.
    
    :param link_name: The name of the link to set the state for
    :param pose: The desired pose, here we set all position and orientation components to 0
    """
    rospy.wait_for_service('/gazebo/set_link_state')
    try:
        set_link_state_service = rospy.ServiceProxy('/gazebo/set_link_state', SetLinkState)
        set_link_state_request = SetLinkStateRequest()
        
        # Set request header information
        set_link_state_request.header = Header()
        set_link_state_request.header.stamp = rospy.Time.now()
        set_link_state_request.header.frame_id = 'world'
        
        # Set the link name
        set_link_state_request.link_name = link_name
        
        # Set the target pose
        set_link_state_request.pose = pose
        
        # Set the angular and linear velocities to 0
        set_link_state_request.twist.linear.x = 0
        set_link_state_request.twist.linear.y = 0
        set_link_state_request.twist.linear.z = 0
        set_link_state_request.twist.angular.x = 0
        set_link_state_request.twist.angular.y = 0
        set_link_state_request.twist.angular.z = 0
        
        # Send the request and wait for the response
        response = set_link_state_service(set_link_state_request)
        return response.success
    except rospy.ServiceException as e:
        print("Failed to call set link state service: %s"%e)

if __name__ == '__main__':
    # Initialize the ROS node
    rospy.init_node('keep_link_static_example')
    
    # Set the link name to keep it static
    link_name = 'mobile_base_link'  # Replace with the name of the link you want to keep static
    
    rate = rospy.Rate(10)  # Set the loop frequency to 10Hz
    while not rospy.is_shutdown():
        # Get the current pose of the link
        current_pose = get_current_pose(link_name)
        if current_pose:
            # Create a Pose message with all position and orientation components set to the current values
            pose = Pose()
            pose.position = current_pose.position
            pose.orientation = current_pose.orientation
            
            # Call the function to set the link state
            success = set_link_state(link_name, pose)
            
            # Print the result
            if success:
                print("Link '%s' set to static relative to the world successfully." % link_name)
            else:
                print("Failed to set link '%s' to static relative to the world." % link_name)
        else:
            print("Could not get the current pose of link '%s'." % link_name)
        
        rate.sleep()  # Sleep based on the set loop frequency

但在运行中出现许多类似的报错。报错举例（截取关键行）：
AttributeError: 'SetLinkStateRequest' object has no attribute 'link_name'
AttributeError: 'SetLinkStateRequest' object has no attribute 'header'
AttributeError: 'GetLinkStateResponse' object has no attribute 'position'
AttributeError: 'GetLinkStateResponse' object has no attribute 'pose'

完整报错举例如下：

Traceback (most recent call last):
  File "/home/hyh/catkin_ws/src/bigmama0225_py/scripts/fix_link_pose0502.py", line 73, in <module>
    current_pose = get_current_pose(link_name)
  File "/home/hyh/catkin_ws/src/bigmama0225_py/scripts/fix_link_pose0502.py", line 22, in get_current_pose
    return response.pose
AttributeError: 'GetLinkStateResponse' object has no attribute 'pose'

Traceback (most recent call last):
  File "/home/hyh/catkin_ws/src/bigmama0225_py/scripts/fix_link_pose0502.py", line 77, in <module>
    pose.position = current_pose.position
AttributeError: 'GetLinkStateResponse' object has no attribute 'position'

Traceback (most recent call last):
  File "/home/hyh/catkin_ws/src/bigmama0225_py/scripts/fix_link_pose0502.py", line 81, in <module>
    success = set_link_state(link_name, pose)
  File "/home/hyh/catkin_ws/src/bigmama0225_py/scripts/fix_link_pose0502.py", line 39, in set_link_state
    set_link_state_request.header = Header()
AttributeError: 'SetLinkStateRequest' object has no attribute 'header'

Traceback (most recent call last):
  File "/home/hyh/catkin_ws/src/bigmama0225_py/scripts/fix_link_pose0502.py", line 82, in <module>
    success = set_link_state(link_name, pose)
  File "/home/hyh/catkin_ws/src/bigmama0225_py/scripts/fix_link_pose0502.py", line 45, in set_link_state
    set_link_state_request.link_name = link_name
AttributeError: 'SetLinkStateRequest' object has no attribute 'link_name'

都是致命报错，但我还是觉得该思路是最有希望的道路，因为之前试着用Python调用set_model_state能成功实现让机器人整体模型frame悬在空中（gravity=true条件下），下面附上调set_model_state服务的代码：

#!/usr/bin/env python
#coding=utf-8

from gazebo_msgs.srv import GetModelState, SetModelState
import rospy
from gazebo_msgs.msg import ModelState
from geometry_msgs.msg import Pose, Point, Quaternion
from gazebo_msgs.srv import *

# Initialize ROS node
rospy.init_node('set_model_state_fixed')

# Wait for Gazebo services to start
rospy.wait_for_service('/gazebo/get_model_state')
rospy.wait_for_service('/gazebo/set_model_state')

# Create GetModelState service proxy
get_model_state_proxy = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)

# Create SetModelState service proxy
set_model_state_proxy = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)

# Get the current state of the model
model_name = 'robot'  # Replace with your model name
reference_frame= 'world'
current_state = get_model_state_proxy(model_name,reference_frame)

# Set the new model state to keep the model fixed in its current position
model_state = ModelState()
model_state.model_name = model_name
model_state.pose = current_state.pose  # Keep the current position
# Set the linear and angular velocities of the model to zero
#model_state.twist = Twist(linear=Vector3(x=0.0, y=0.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=0.0))
model_state.twist.linear.x = 0.0
model_state.twist.linear.y = 0.0
model_state.twist.linear.z = 0.0
model_state.twist.angular.x = 0.0
model_state.twist.angular.y = 0.0
model_state.twist.angular.z = 0.0
model_state.reference_frame = 'world'

# Call the SetModelState service to set the model state
set_model_state_proxy(model_state)

# Output result
print("Model state set successfully, model is now fixed in its current position.")

# To ensure that the model stays fixed in its current position in future updates, we need to continuously call the SetModelState service
# This can be done using a loop or other logic based on your specific requirements
while not rospy.is_shutdown():
    set_model_state_proxy(model_state)
    rospy.sleep(0.01)  # Sleep for 0.01 second, adjust frequency as needed

然后我通过rosservice call 验证不是版本不适配或环境缺陷的问题，具体操作如下：

终端输入 rostopic list
返回里包含有/gazebo/set_link_state, 完整返回字段如下：

/arm_position_controller/command
/arm_position_controller/follow_joint_trajectory/cancel
/arm_position_controller/follow_joint_trajectory/feedback
/arm_position_controller/follow_joint_trajectory/goal
/arm_position_controller/follow_joint_trajectory/result
/arm_position_controller/follow_joint_trajectory/status
/arm_position_controller/state
/attached_collision_object
/clock
/collision_object
/execute_trajectory/cancel
/execute_trajectory/feedback
/execute_trajectory/goal
/execute_trajectory/result
/execute_trajectory/status
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/joint_states
/move_group/cancel
/move_group/display_contacts
/move_group/display_cost_sources
/move_group/display_grasp_markers
/move_group/display_planned_path
/move_group/feedback
/move_group/goal
/move_group/monitored_planning_scene
/move_group/motion_plan_request
/move_group/ompl/parameter_descriptions
/move_group/ompl/parameter_updates
/move_group/plan_execution/parameter_descriptions
/move_group/plan_execution/parameter_updates
/move_group/planning_scene_monitor/parameter_descriptions
/move_group/planning_scene_monitor/parameter_updates
/move_group/result
/move_group/sense_for_plan/parameter_descriptions
/move_group/sense_for_plan/parameter_updates
/move_group/status
/move_group/trajectory_execution/parameter_descriptions
/move_group/trajectory_execution/parameter_updates
/pickup/cancel
/pickup/feedback
/pickup/goal
/pickup/result
/pickup/status
/place/cancel
/place/feedback
/place/goal
/place/result
/place/status
/planning_scene
/planning_scene_world
/recognized_object_array
/rosout
/rosout_agg
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/feedback
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full
/rviz_ubuntu_38423_7706691175209945188/motionplanning_planning_scene_monitor/parameter_descriptions
/rviz_ubuntu_38423_7706691175209945188/motionplanning_planning_scene_monitor/parameter_updates
/tf
/tf_static
/trajectory_execution_event

然后终端单纯命令调用 rosservice call gazebo/get_link_state '{link_name: Link_3}'

link_state: 
  link_name: "Link_3"
  pose: 
    position: 
      x: -0.0169728441434
      y: -0.239294662543
      z: 1.27101571602
    orientation: 
      x: 0.506675069487
      y: -0.506605807831
      z: 0.493296103713
      w: -0.493244243244
  twist: 
    linear: 
      x: 0.00139357024937
      y: 0.000851167082131
      z: -0.00139001604216
    angular: 
      x: -0.00435862833302
      y: 0.00725633894658
      z: 6.4154873921e-05
  reference_frame: ''
success: True
status_message: "GetLinkState: got state"

最后发现问题的关键有两个，一个是get_link_state得到的response它包含了pose和twist的信息，其中pose是我们想要的，但不能直接用return response.pose 的暴力方式提取pose，我后面是使用current_state向量空间先把get_link_state得到的整个response装进来，然后新定义一个pose（）类的向量poseoutput，把current_state里面pose下的值一个个填进poseoutput的7个变量，后面再把poseoutput的结果喂给set_link_state的第二个自变量poseinput。

第二个关键问题是用set_link_state_request给set_link_state服务输入link_name，reference_frame, pose，twist等参数时，得再在中间插入个link_state作为playfield，这点跟用get_link_state的时候不同。

通宵跑通了，下面是改好的两款，一个是get一次然后循环set，第二个是循环get and set：

#!/usr/bin/env python
#coding=utf-8

import rospy
import geometry_msgs.msg
from gazebo_msgs.msg import LinkState
from gazebo_msgs.srv import GetLinkState, GetLinkStateRequest, SetLinkState, SetLinkStateRequest
from geometry_msgs.msg import Pose, Point, Quaternion, Twist
from std_msgs.msg import Header

def get_current_state(link_name):
    """
    Get the current pose of the specified link.
    
    :param link_name: The name of the link to get the pose for
    :return: The current pose of the link as a Pose object
    """
    rospy.wait_for_service('/gazebo/get_link_state')
    try:
        get_link_state_service = rospy.ServiceProxy('/gazebo/get_link_state', GetLinkState)
        get_link_state_request = GetLinkStateRequest()
        get_link_state_request.link_name = link_name
        get_link_state_request.reference_frame = 'world'
        response = get_link_state_service(get_link_state_request)
        return response
    except rospy.ServiceException as e:
        print("Failed to call get link state service: %s"%e)

def set_link_state(link_name, poseinput):
    """
    Set the state of the specified link to keep it static relative to the world.
    
    :param link_name: The name of the link to set the state for
    :param pose: The desired pose, here we set all position and orientation components to 0
    """
    rospy.wait_for_service('/gazebo/set_link_state')
    try:
        set_link_state_service = rospy.ServiceProxy('/gazebo/set_link_state', SetLinkState)
        set_link_state_request = SetLinkStateRequest()
        # Set the link name
        set_link_state_request.link_state.link_name = link_name
        set_link_state_request.link_state.reference_frame = 'world'
        
        # Set the target pose
        set_link_state_request.link_state.pose = poseinput
        
        # Set the angular and linear velocities to 0
        set_link_state_request.link_state.twist = geometry_msgs.msg.Twist(linear= geometry_msgs.msg.Vector3( x = 0, y = 0, z = 0), angular = geometry_msgs.msg.Vector3( x = 0, y = 0, z = 0))
        #set_link_state_request.twist.linear.x = 0
        #set_link_state_request.twist.linear.y = 0
        #set_link_state_request.twist.linear.z = 0
        #set_link_state_request.twist.angular.x = 0
        #set_link_state_request.twist.angular.y = 0
        #set_link_state_request.twist.angular.z = 0
        
        # Send the request and wait for the response
        result = set_link_state_service(set_link_state_request)
        return result
    except rospy.ServiceException as e:
        print("Failed to call set link state service: %s"%e)

if __name__ == '__main__':
    # Initialize the ROS node
    rospy.init_node('keep_link_static_example')
    
    # Set the link name to keep it static
    link_name = 'Link_3'  # Replace with the name of the link you want to keep static
    # Get the current pose of the link
    current_state = get_current_state(link_name)
    poseoutput = Pose()
    poseoutput.position.x = current_state.link_state.pose.position.x
    poseoutput.position.y = current_state.link_state.pose.position.y
    poseoutput.position.z = current_state.link_state.pose.position.z
    poseoutput.orientation.w = current_state.link_state.pose.orientation.w 
    poseoutput.orientation.x = current_state.link_state.pose.orientation.x 
    poseoutput.orientation.y = current_state.link_state.pose.orientation.y 
    poseoutput.orientation.z = current_state.link_state.pose.orientation.z 
    rate = rospy.Rate(50)  # Set the loop frequency to 50Hz
    while not rospy.is_shutdown():

        if current_state:
            # Create a Pose message with all position and orientation components set to the current values          
            # Call the function to set the link state
            success = set_link_state(link_name, poseoutput)
            
            # Print the result
            if success:
                print("Link '%s' set to static relative to the world successfully." % link_name)
            else:
                print("Failed to set link '%s' to static relative to the world." % link_name)
        else:
            print("Could not get the current pose of link '%s'." % link_name)
        
        rate.sleep()  # Sleep based on the set loop frequency

第二个：

#!/usr/bin/env python
#coding=utf-8

from gazebo_msgs.srv import GetLinkState, SetLinkState
import rospy
from gazebo_msgs.msg import LinkState
from geometry_msgs.msg import Pose, Point, Quaternion
from gazebo_msgs.srv import *

# Initialize ROS node
rospy.init_node('set_link_state_fixed')

# Wait for Gazebo services to start
rospy.wait_for_service('/gazebo/get_link_state')
rospy.wait_for_service('/gazebo/set_link_state')

# Create GetLinkState service proxy
get_link_state_proxy = rospy.ServiceProxy('/gazebo/get_link_state', GetLinkState)

# Create SetLinkState service proxy
set_link_state_proxy = rospy.ServiceProxy('/gazebo/set_link_state', SetLinkState)

# Get the current state of the link
link_name = 'Link_3'  # Replace with your link name
reference_frame= 'world'
current_state = get_link_state_proxy(link_name,reference_frame)

# Set the new link state to keep the link fixed in its current position
link_state = LinkState()
link_state.link_name = link_name
link_state.pose = current_state.link_state.pose  # Keep the current position
# Set the linear and angular velocities of the link to zero
#link_state.twist = Twist(linear=Vector3(x=0.0, y=0.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=0.0))
link_state.twist.linear.x = 0.0
link_state.twist.linear.y = 0.0
link_state.twist.linear.z = 0.0
link_state.twist.angular.x = 0.0
link_state.twist.angular.y = 0.0
link_state.twist.angular.z = 0.0
link_state.reference_frame = 'world'

# Call the SetlinkState service to set the link state
set_link_state_proxy(link_state)

# Output result
print("link state set successfully, link is now fixed in its current position.")

# To ensure that the link stays fixed in its current position in future updates, we need to continuously call the SetlinkState service
# This can be done using a loop or other logic based on your specific requirements
while not rospy.is_shutdown():
    set_link_state_proxy(link_state)
    rospy.sleep(0.002)  # Sleep for 0.002 second, adjust frequency as needed

值得注意的点是link_name赋值的时候并不是如官网说的那样"model_name::link_name"，而是直接给你link的名字，所以整个仿真世界里最好别有重名的。

第一个跑起来是这样的反应，极其啰嗦：