Python调用'/gazebo/set_link_state'发生AttributeError

Landson Housebigger

**## 标题：**Python代码调用get_link_state和set_link_state

**### 背景：**在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):
    """
    获取指定链接的当前位姿。
    
    :param link_name: 要获取位姿的链接名称
    :return: 当前位姿的Pose对象
    """
    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("获取链接状态服务调用失败: %s"%e)

def set_link_state(link_name, pose):
    """
    设置链接状态，使指定的链接相对于世界保持静止。
    
    :param link_name: 要设置状态的链接名称
    :param pose: 设置的位姿，这里我们将位置和方向都设置为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_link_state_request.header = Header()
        set_link_state_request.header.stamp = rospy.Time.now()
        set_link_state_request.header.frame_id = 'world'
        
        # 设置链接名称
        set_link_state_request.link_name = link_name
        
        # 设置目标位姿
        set_link_state_request.pose = pose
        
        # 设置角速度和线速度为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
        
        # 发送请求并等待响应
        response = set_link_state_service(set_link_state_request)
        return response.success
    except rospy.ServiceException as e:
        print("设置链接状态服务调用失败: %s"%e)

if __name__ == '__main__':
    # 初始化ROS节点
    rospy.init_node('keep_link_static_example')
    
    # 设置要保持静止的链接名称
    link_name = 'mobile_link'  # 这里替换为目标链接名称
    
    rate = rospy.Rate(10)  # 设置循环频率为10Hz
    while not rospy.is_shutdown():
        # 获取当前位姿
        current_pose = get_current_pose(link_name)
        if current_pose:
            # 创建一个Pose消息，将位置和方向都设置为当前值
            pose = Pose()
            pose.position = current_pose.position
            pose.orientation = current_pose.orientation
            
            # 调用函数设置链接状态
            success = set_link_state(link_name, pose)
            
            # 打印结果
            if success:
                print("链接 '%s' 设置为相对于世界静止成功。" % link_name)
            else:
                print("链接 '%s' 设置为相对于世界静止失败。" % link_name)
        else:
            print("无法获取链接 '%s' 的当前位姿。" % link_name)
        
        rate.sleep()  # 根据设定的循环频率休眠

附英文版

#!/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_link'  # Replace with the name of the link  intended to be 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

### 尝试过的解决方法：
代码部分试着删过报错中提到的各个参量，结果参量全删完了还用个毛。然后仿真部分为了让特定的link固定在管壁上试过(ros-melodic环境下）apply_body_wrench，grasp_fix_plugin，摩擦mu1mu2给到100了还是会滑移，也试过换Ubuntu22.04+RosHumble+gazebo11环境里用 apply_link_force, ros2_grasping(https://github.com/IFRA-Cranfield/ros2_RobotSimulation/tree/foxy/ros2_grasping)，ros2_LinkAttacher(https://github.com/IFRA-Cranfield/IFRA_LinkAttacher)，都是只能在静态时稳定，base_link那端一抬起来整个模型就飞掉最后坍缩到世界原点处。
最后还是回到ROS-melodic里面舒服点，我这VM17的虚拟机跑ros-humble跑的提心吊胆的，时不时就给你崩一下子真受不了。在座的各位前辈大神有没有用过set_link_state服务，救救我谢谢了。

Landson Housebigger

@3436845452 问题里提到的robot长这个样，想人为固定住末端mobile_link（黄色），然后通过相对运动把base_link（绿色）抬起来翻过去。

Landson Housebigger

@3436845452 终端输入 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

Landson Housebigger

@3436845452 终端单纯命令调用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"

Landson Housebigger

通宵跑通了，下面是改好的两款，一个是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