Gazebo中使用set_link_state来改变物体位置

Gazebo中，可以在运行的时候使用/gazebo/set_link_state服务来改变物体的位置。

首先对于要修改位置的物体，一定要记得在.xacro文件中的<gazebo>标签下加入<kinematic>1，表示只进行运动学计算，不涉及运动学，比如

<gazebo reference="camera_link">
    <material>Gazebo/Black</material>
    <kinematic>1</kinematic>
</gazebo>

这样之后使用服务来改变物体位置才会成功，不然会被拉回原来的位置。

之后可以利用话题来进行位置的设定：

	ros::ServiceClient client = node_handle.serviceClient<gazebo_msgs::SetLinkState>("/gazebo/set_link_state");

    // the first camera pos
    gazebo_msgs::SetLinkState objstate;

    objstate.request.link_state.link_name = "camera_link";
    objstate.request.link_state.pose.position.x = 0.0;
    objstate.request.link_state.pose.position.y = 0.0;
    objstate.request.link_state.pose.position.z = 0.5;
    objstate.request.link_state.pose.orientation.w = 1;
    objstate.request.link_state.pose.orientation.x = 0;
    objstate.request.link_state.pose.orientation.y = 0;
    objstate.request.link_state.pose.orientation.z = 0;
    objstate.request.link_state.twist.linear.x = 0.0;
    objstate.request.link_state.twist.linear.y = 0.0;
    objstate.request.link_state.twist.linear.z = 0.0;
    objstate.request.link_state.twist.angular.x = 0.0;
    objstate.request.link_state.twist.angular.y = 0.0;
    objstate.request.link_state.twist.angular.z = 0.0;
    objstate.request.link_state.reference_frame = "world";
    client.call(objstate);

也可以使用命令行来设置：

rosservice call gazebo/set_link_state '{link_state: {link_name: camera_link, pose: {position: {x: 0.5, y: 0.5, z: 0.5}}}}'