ROS_Robot/wpr_simulation
1
0
Fork 0
mirror of https://github.com/6-robot/wpr_simulation.git synced 2025-09-15 12:59:01 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

wpb_scene_1

Browse Source
This commit is contained in:
zhangwanjie 2020-12-11 18:58:13 +08:00
parent 5ef6dfd034
commit c5af5c82da
9 changed files with 221 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
README.md
View File

@ -56,7 +56,7 @@ roslaunch wpr_simulation wpb_navigation.launch
物品抓取:
```
roslaunch wpr_simulation wpb_home_table.launch
roslaunch wpr_simulation wpb_table.launch
rosrun wpb_home_tutorials wpb_home_grab_client
```
![wpb_home_table pic](./media/wpb_home_table.png)

94
launch/wpb_scene_1.launch Normal file
View File

@ -0,0 +1,94 @@
<launch>
<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find wpr_simulation)/worlds/robocup_home.world"/>
<arg name="paused" value="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="true"/>
<arg name="recording" value="false"/>
<arg name="debug" value="false"/>
</include>
<!-- Spawn the objects into Gazebo -->
<node name="bed" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/bed.model -x 5.0 -y -3.9 -z 0 -Y 3.14159 -urdf -model bed" />
<node name="sofa" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/sofa.model -x -1.0 -y -3.9 -z 0 -Y 1.57 -urdf -model sofa" />
<node name="tea_table" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/tea_table.model -x -2.1 -y -2.2 -z 0 -Y 1.57 -urdf -model tea_table" />
<node name="bookshelft" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/bookshelft.model -x 2.0 -y -0.55 -z 0 -Y -1.57 -urdf -model bookshelft" />
<node name="kitchen_table" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x -3.5 -y 3.7 -z 0 -Y 1.57 -urdf -model kitchen_table" />
<node name="bottle_0" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/bottles/red_bottle.model -x -3.3 -y 3.55 -z 10.0 -Y 0 -urdf -model bottle_0" />
<node name="bottle_1" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/bottles/green_bottle.model -x -3.6 -y 3.55 -z 10.0 -Y 0 -urdf -model bottle_1" />
<node name="cupboard_0" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/cupboard.model -x -2.0 -y 0.7 -z 0 -Y 1.57 -urdf -model cupboard_0" />
<node name="cupboard_1" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/cupboard.model -x -1.3 -y 3.7 -z 0 -Y -1.57 -urdf -model cupboard_1" />
<node name="dinning_table_0" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x 1.5 -y 1.5 -z 0 -Y 1.57 -urdf -model dinning_table_0" />
<node name="dinning_table_1" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x 1.5 -y 2.0 -z 0 -Y 1.57 -urdf -model dinning_table_1" />
<node name="dinning_table_2" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x 2.7 -y 1.5 -z 0 -Y 1.57 -urdf -model dinning_table_2" />
<node name="dinning_table_3" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x 2.7 -y 2.0 -z 0 -Y 1.57 -urdf -model dinning_table_3" />
<node name="chair_0" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/chair.model -x 1.5 -y 1.2 -z 0 -Y 1.57 -urdf -model chair_0" />
<node name="chair_1" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/chair.model -x 1.5 -y 2.3 -z 0 -Y -1.57 -urdf -model chair_1" />
<node name="chair_2" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/chair.model -x 2.7 -y 1.2 -z 0 -Y 1.57 -urdf -model chair_2" />
<node name="chair_3" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/chair.model -x 2.7 -y 2.3 -z 0 -Y -1.57 -urdf -model chair_3" />
<!-- Spawn a robot into Gazebo -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/wpb_home_mani.model -urdf -x -6.0 -y -0.5 -model wpb_home_mani" />
<!-- load the controllers of WPB_HOME -->
<include file="$(find wpr_simulation)/launch/wpb_home_controllers.launch"/>
<!-- wpb_home_behaviors -->
<node pkg="wpb_home_behaviors" type="wpb_home_grab_server" name="wpb_home_grab_server" />
<!-- wpb_home_objects_3d -->
<node pkg="wpb_home_behaviors" type="wpb_home_objects_3d" name="wpb_home_objects_3d" output="screen">
<param name="topic" type="string" value="/kinect2/sd/points"/>
</node>
<!-- wpb_home_grab_action -->
<node pkg="wpb_home_behaviors" type="wpb_home_grab_action" name="wpb_home_grab_action">
<rosparam file="$(find wpb_home_bringup)/config/wpb_home.yaml" command="load" />
</node>
<!-- Run the map server -->
<node name="map_server" pkg="map_server" type="map_server" args="$(find wpr_simulation)/maps/map.yaml"/>
<!--- Run AMCL -->
<include file="$(find wpb_home_tutorials)/nav_lidar/amcl_omni.launch" />
<!--- Run move base -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/local_costmap_params.yaml" command="load" />
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/global_costmap_params.yaml" command="load" />
<param name="base_global_planner" value="global_planner/GlobalPlanner" />
<param name="use_dijkstra" value="true"/>
<param name="base_local_planner" value="wpbh_local_planner/WpbhLocalPlanner" />
<param name= "controller_frequency" value="10" type="double"/>
</node>
<!-- Map tools -->
<node pkg="waterplus_map_tools" type="wp_manager" name="wp_manager" output="screen" />
<!-- Navi server -->
<node pkg="waterplus_map_tools" type="wp_navi_server" name="wp_navi_server" output="screen" />
<!-- RViz and TF tree -->
<arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home.urdf"/>
<arg name="gui" default="false" />
<arg name="rvizconfig" default="$(find wpr_simulation)/rviz/map_tool.rviz" />
<param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
<param name="use_gui" value="$(arg gui)"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" >
<rosparam command="load" file="$(find wpb_home_bringup)/config/wpb_home.yaml" />
</node>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
</launch>

2
launch/wpb_simple.launch
View File

@ -11,7 +11,7 @@
<!-- Spawn the objects into Gazebo -->
<node name="bookshelft" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/bookshelft.model -x 3.0 -y 0.2 -z 0 -Y 3.14159 -urdf -model bookshelft" />
<node name="bottle" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/bottle.model -x 2.8 -y 0 -z 0.6 -Y 0 -urdf -model bottle" />
<node name="bottle" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/bottles/red_bottle.model -x 2.8 -y 0 -z 0.6 -Y 0 -urdf -model red_bottle" />
<!-- Spawn a robot into Gazebo -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/wpb_home.model -urdf -model wpb_home" />

14
launch/wpb_home_table.launch → launch/wpb_table.launch
View File

@ -11,7 +11,8 @@
<!-- Spawn the objects into Gazebo -->
<node name="table" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x 1.2 -y 0.0 -z 0 -Y 0 -urdf -model table" />
<node name="bottle" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/bottle.model -x 1.1 -y 0.3 -z 1.0 -Y 0 -urdf -model bottle" />
<node name="red_bottle" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/bottles/red_bottle.model -x 1.1 -y 0.3 -z 2.0 -Y 0 -urdf -model red_bottle" />
<node name="green_bottle" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/bottles/green_bottle.model -x 1.1 -y -0.2 -z 2.0 -Y 0 -urdf -model green_bottle" />
<!-- Spawn a robot into Gazebo -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/wpb_home_mani.model -urdf -model wpb_home_mani" />
@ -19,8 +20,19 @@
<!-- load the controllers of WPB_HOME -->
<include file="$(find wpr_simulation)/launch/wpb_home_controllers.launch"/>
<!-- wpb_home_grab_server -->
<node pkg="wpb_home_behaviors" type="wpb_home_grab_server" name="wpb_home_grab_server" />
<!-- wpb_home_objects_3d -->
<node pkg="wpb_home_behaviors" type="wpb_home_objects_3d" name="wpb_home_objects_3d" output="screen">
<param name="topic" type="string" value="/kinect2/sd/points"/>
</node>
<!-- wpb_home_grab_action -->
<node pkg="wpb_home_behaviors" type="wpb_home_grab_action" name="wpb_home_grab_action">
<rosparam file="$(find wpb_home_bringup)/config/wpb_home.yaml" command="load" />
</node>
<!-- rviz -->
<arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home_mani.urdf"/>
<arg name="gui" default="false" />

2
launch/wpr1_simple.launch
View File

@ -11,7 +11,7 @@
<!-- Spawn the objects into Gazebo -->
<node name="bookshelft" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/bookshelft.model -x 3.0 -y 0.2 -z 0 -Y 3.14159 -urdf -model bookshelft" />
<node name="bottle" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/bottle.model -x 2.8 -y 0 -z 0.6 -Y 0 -urdf -model bottle" />
<node name="bottle" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/bottles/red_bottle.model -x 2.8 -y 0 -z 0.6 -Y 0 -urdf -model red_bottle" />
<!-- Spawn a robot into Gazebo -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/wpr1.model -urdf -model wpr1" />

57
models/bottles/green_bottle.model Normal file
View File

@ -0,0 +1,57 @@
<?xml version="1.0"?>
<robot name="green_bottle">
<!-- base -->
<link name="base_link">
<visual>
<geometry>
<box size="0.01 0.01 0.001" />
</geometry>
<origin rpy = "0 0 0" xyz = "0 0 0"/>
</visual>
</link>
<!-- body -->
<link name = "body_link">
<visual>
<geometry>
<mesh filename="package://wpr_simulation/meshes/bottle.stl" scale="1 1 1"/>
</geometry>
<origin rpy = "1.57 0 0" xyz = "-0.03 0.03 0"/>
</visual>
<collision>
<origin xyz="0 0 0.1" rpy="0 0 0" />
<geometry>
<cylinder length="0.2" radius="0.03"/>
</geometry>
</collision>
<collision>
<origin xyz="0 0 0.1" rpy="0 0 0" />
<geometry>
<cylinder length="0.2" radius="0.005"/>
</geometry>
</collision>
<inertial>
<mass value="0.01"/>
<inertia ixx="0.538" ixy="0.0" ixz="0.0" iyy="0.538" iyz="0.0" izz="0.076"/>
</inertial>
</link>
<joint name = "base_to_body" type = "fixed">
<parent link = "base_link"/>
<child link = "body_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/> <!--pos-->
</joint>
<gazebo reference="body_link">
<material>Gazebo/Green</material>
</gazebo>
<gazebo reference="body_link">
<kp>1000000.0</kp>
<kd>100.0</kd>
<mu1>50.0</mu1>
<mu2>50.0</mu2>
<maxVel>1.0</maxVel>
<minDepth>0.001</minDepth>
</gazebo>
</robot>

27
models/bottle.model → models/bottles/red_bottle.model
View File

@ -1,5 +1,5 @@
<?xml version="1.0"?>
<robot name="bottle">
<robot name="red_bottle">
<!-- base -->
<link name="base_link">
@ -45,22 +45,13 @@
<material>Gazebo/Red</material>
</gazebo>
<link name = "handle_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="0.005" radius="0.04"/>
</geometry>
</collision>
<inertial>
<mass value="0.00001"/>
<inertia ixx="0.000001" ixy="0.0" ixz="0.0" iyy="0.000001" iyz="0.0" izz="0.000001"/>
</inertial>
</link>
<joint name = "base_to_handle" type = "fixed">
<parent link = "base_link"/>
<child link = "handle_link"/>
<origin rpy="0 0 0" xyz="0 0 0.09"/> <!--pos-->
</joint>
<gazebo reference="body_link">
<kp>1000000.0</kp>
<kd>100.0</kd>
<mu1>50.0</mu1>
<mu2>50.0</mu2>
<maxVel>1.0</maxVel>
<minDepth>0.001</minDepth>
</gazebo>
</robot>

32
models/wpb_home_mani.model
View File

@ -239,7 +239,13 @@
</inertial>
</link>
<gazebo reference="mani_left_finger">
<material>Gazebo/Grey</material>
<material>Gazebo/Grey</material>
<kp>1000000.0</kp>
<kd>100.0</kd>
<mu1>30.0</mu1>
<mu2>30.0</mu2>
<maxVel>1.0</maxVel>
<minDepth>0.001</minDepth>
</gazebo>
<joint name = "palm_left_finger" type = "revolute">
<parent link = "mani_forearm"/>
@ -285,7 +291,13 @@
</inertial>
</link>
<gazebo reference="mani_left_finger_tip">
<material>Gazebo/Grey</material>
<material>Gazebo/Grey</material>
<kp>1000000.0</kp>
<kd>100.0</kd>
<mu1>30.0</mu1>
<mu2>30.0</mu2>
<maxVel>1.0</maxVel>
<minDepth>0.001</minDepth>
</gazebo>
<joint name = "left_finger_tip" type = "revolute">
<parent link = "mani_left_finger"/>
@ -326,7 +338,13 @@
</inertial>
</link>
<gazebo reference="mani_right_finger">
<material>Gazebo/Grey</material>
<material>Gazebo/Grey</material>
<kp>1000000.0</kp>
<kd>100.0</kd>
<mu1>30.0</mu1>
<mu2>30.0</mu2>
<maxVel>1.0</maxVel>
<minDepth>0.001</minDepth>
</gazebo>
<joint name = "palm_right_finger" type = "revolute">
<parent link = "mani_forearm"/>
@ -373,7 +391,13 @@
</inertial>
</link>
<gazebo reference="mani_right_finger_tip">
<material>Gazebo/Grey</material>
<material>Gazebo/Grey</material>
<kp>1000000.0</kp>
<kd>100.0</kd>
<mu1>30.0</mu1>
<mu2>30.0</mu2>
<maxVel>1.0</maxVel>
<minDepth>0.001</minDepth>
</gazebo>
<joint name = "right_finger_tip" type = "revolute">
<parent link = "mani_right_finger"/>

28
src/wpb_home_sim.cpp
View File

@ -97,15 +97,21 @@ void ManiHeight(float inHeight)
void ManiCtrlCallback(const sensor_msgs::JointState::ConstPtr& msg)
{
// int nNumJoint = msg->position.size();
// for(int i=0;i<nNumJoint;i++)
// {
// ROS_INFO("[wpb_home] %d - %s = %.2f vel= %.2f", i, msg->name[i].c_str(),msg->position[i],msg->velocity[i]);
// }
//高度升降
ManiHeight(msg->position[0]);
//手爪
ManiGripper(msg->position[1]);
int nNumJoint = msg->position.size();
for(int i=0;i<nNumJoint;i++)
{
//ROS_INFO("[wpb_home] %d - %s = %.2f vel= %.2f", i, msg->name[i].c_str(),msg->position[i],msg->velocity[i]);
if(msg->name[i] == "lift")
{
//高度升降
ManiHeight(msg->position[i]);
}
if(msg->name[i] == "gripper")
{
//手爪
ManiGripper(msg->position[i]);
}
}
}
geometry_msgs::Pose2D CalDiffPose(geometry_msgs::Pose2D inMapPose)
@ -177,8 +183,8 @@ int main(int argc, char** argv)
{
listener.lookupTransform("/pose_reset","/base_footprint",ros::Time(0),tf_diff);
pose_diff_msg.x = tf_diff.getOrigin().x();
pose_diff_msg.y = tf_diff.getOrigin().y();
pose_diff_msg.x = tf_diff.getOrigin().x()*0.9;
pose_diff_msg.y = tf_diff.getOrigin().y()*0.9;
tf::Quaternion pose_quat = tf_diff.getRotation ();
float pose_yaw = tf::getYaw(pose_quat);
pose_diff_msg.theta = pose_yaw - pose_reset.theta;