Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP

Loading…

Elevators in ipa-apartment and object QRCode #4

Merged
merged 4 commits into from

2 participants

This page is out of date. Refresh to see the latest.
View
5 cob_gazebo_objects/config/ipa-apartment/object_locations.yaml
@@ -63,3 +63,8 @@ couch:
position: [3.6,0.6,0]
orientation: [0, 0,-1.57079]
+QRCode:
+ model_type: urdf.xacro
+ position: [-1.34,-1.7,2]
+ orientation: [0,0,0]
+
View
69 cob_gazebo_objects/objects/QRCode.urdf.xacro
@@ -0,0 +1,69 @@
+<?xml version="1.0"?>
+<robot name="QRCode"
+ xmlns:xi="http://www.w3.org/2001/XInclude"
+ xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
+ xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
+ xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+ xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
+ xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
+ xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
+ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+ xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
+ xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
+ xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+ xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics"
+>
+
+
+<!-- dummy_link -->
+ <link name="dummy_link">
+ <inertial>
+ <origin xyz="0 0 0" />
+ <mass value="5.0" />
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="0.00 0.00 0.00" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="0.00 0.00 0.00" />
+ </geometry>
+ </collision>
+ </link>
+
+ <link name="QRCode">
+ <inertial>
+ <origin xyz="0 0 0" />
+ <mass value="5.0" />
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="0.01 0.2 0.2" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="0.01 0.1 2.5" />
+ </geometry>
+ </collision>
+ </link>
+
+ <gazebo reference="QRCode">
+ <material>IPA/QRCode</material>
+ </gazebo>
+
+ <joint name="joint_QRCode" type="fixed">
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <parent link="dummy_link" />
+ <child link="QRCode" />
+ </joint>
+
+</robot>
View
14 cob_gazebo_worlds/Media/materials/scripts/ipa.material
@@ -28,6 +28,20 @@ material IPA/Care-O-bot
}
}
+material IPA/QRCode
+{
+ technique
+ {
+ pass
+ {
+ texture_unit
+ {
+ texture QRCode.png
+ }
+ }
+ }
+}
+
material IPA/Future
{
technique
View
BIN  cob_gazebo_worlds/Media/materials/textures/QRCode.png
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
View
505 cob_gazebo_worlds/common/worlds/ipa-apartment.urdf.xacro
@@ -408,6 +408,511 @@ Setting pose: 1.969 -4.262 0.087
<child link="door_01" />
</joint>
+ <!-- wall ext -->
+ <link name="wall_ext">
+ <inertial>
+ <origin xyz="0 0 0" />
+ <mass value="5.0" />
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="4.6 0.04 ${size_z_wall1}" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="4.6 0.04 ${size_z_wall1}" />
+ </geometry>
+ </collision>
+ </link>
+
+ <gazebo reference="wall_ext">
+ <material>IPA/WallApartment</material>
+ </gazebo>
+
+ <joint name="joint_wall_ext" type="fixed">
+ <origin xyz="${-size_total_x/2+dist_axis_x-wall_depth-2.3} ${-0.68-dist_axis_y} ${size_z_wall1/2}" rpy="0 0 ${-dist_axis_yall}" />
+ <parent link="dummy_link" />
+ <child link="wall_ext" />
+ </joint>
+
+
+ <!-- wall ext -->
+ <link name="wall_ext2">
+ <inertial>
+ <origin xyz="0 0 0" />
+ <mass value="5.0" />
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="4.6 0.04 ${size_z_wall1}" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="4.6 0.04 ${size_z_wall1}" />
+ </geometry>
+ </collision>
+ </link>
+
+ <gazebo reference="wall_ext2">
+ <material>IPA/WallApartment</material>
+ </gazebo>
+
+ <joint name="joint_wall_ext2" type="fixed">
+ <origin xyz="${-size_total_x/2+dist_axis_x-wall_depth-2.3} ${-0.68-dist_axis_y+1.6} ${size_z_wall1/2}" rpy="0 0 ${-dist_axis_yall}" />
+ <parent link="dummy_link" />
+ <child link="wall_ext2" />
+ </joint>
+
+ <!-- wall ext3 -->
+ <link name="wall_ext3">
+ <inertial>
+ <origin xyz="0 0 0" />
+ <mass value="5.0" />
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="0.04 1.9 ${size_z_wall1}" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="0.04 1.9 ${size_z_wall1}" />
+ </geometry>
+ </collision>
+ </link>
+
+ <gazebo reference="wall_ext3">
+ <material>IPA/WallApartment</material>
+ </gazebo>
+
+ <joint name="joint_wall_ext3" type="fixed">
+ <origin xyz="${-size_total_x/2+dist_axis_x-wall_depth-4.6-0.68} ${-0.68-dist_axis_y-0.65} ${size_z_wall1/2}" rpy="0 0 ${-dist_axis_yall-0.7853}" />
+ <parent link="dummy_link" />
+ <child link="wall_ext3" />
+ </joint>
+
+ <!-- wall ext4 -->
+ <link name="wall_ext4">
+ <inertial>
+ <origin xyz="0 0 0" />
+ <mass value="5.0" />
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="0.04 0.72 ${size_z_wall1}" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="0.04 0.72 ${size_z_wall1}" />
+ </geometry>
+ </collision>
+ </link>
+
+ <gazebo reference="wall_ext4">
+ <material>IPA/WallApartment</material>
+ </gazebo>
+
+ <joint name="joint_wall_ext4" type="fixed">
+ <origin xyz="${-size_total_x/2+dist_axis_x-wall_depth-4.6-0.68-1.54} ${-0.68-dist_axis_y-0.65-1.54} ${size_z_wall1/2}" rpy="0 0 ${-dist_axis_yall-0.7853}" />
+ <parent link="dummy_link" />
+ <child link="wall_ext4" />
+ </joint>
+
+
+
+ <!--elevator buttons-->
+
+ <link name="elevator_button1">
+ <inertial>
+ <origin xyz="0 0 0" />
+ <mass value="1.0" />
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="0.01 0.05 0.05" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="0.01 0.05 0.05" />
+ </geometry>
+ </collision>
+ </link>
+
+
+ <joint name="joint_elevator_button1" type="prismatic">
+ <origin xyz="${-0.025} ${0.025} ${-size_z_wall1/2+1.035}" rpy="0 0 0" />
+ <parent link="wall_ext4" />
+ <child link="elevator_button1" />
+
+ <calibration rising="0.0"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ <limit effort="100" velocity="1.0" lower="0" upper="0" />
+ <safety_controller k_velocity="10"/>
+
+ </joint>
+
+ <gazebo reference="elevator_button1">
+ <selfCollide>true</selfCollide>
+ <sensor:contact name="elevator_button1_contact_sensor">
+ <geom>elevator_button1_geom</geom>
+ <updateRate>10</updateRate>
+ <controller:gazebo_ros_bumper name="elevator_button1_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>10</updateRate>
+ <frameName>elevator_button1</frameName>
+ <bumperTopicName>elevator_button1_bumper</bumperTopicName>
+ <interface:bumper name="elevator_button1_bumper_iface" />
+ </controller:gazebo_ros_bumper>
+ </sensor:contact>
+ <material>Gazebo/Red</material>
+ <!--turnGravityOff>false</turnGravityOff-->
+ </gazebo>
+
+
+ <link name="elevator_button2">
+ <inertial>
+ <origin xyz="0 0 0" />
+ <mass value="1.0" />
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="0.01 0.05 0.05" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="0.01 0.05 0.05" />
+ </geometry>
+ </collision>
+ </link>
+
+
+ <joint name="joint_elevator_button2" type="prismatic">
+ <origin xyz="${-0.025} ${0.025} ${-size_z_wall1/2+1.095}" rpy="0 0 0" />
+ <parent link="wall_ext4" />
+ <child link="elevator_button2" />
+
+ <calibration rising="0.0"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ <limit effort="100" velocity="1.0" lower="0" upper="0" />
+ <safety_controller k_velocity="10"/>
+ </joint>
+
+
+
+ <gazebo reference="elevator_button2">
+ <selfCollide>true</selfCollide>
+ <sensor:contact name="elevator_button2_contact_sensor">
+ <geom>elevator_button2_geom</geom>
+ <updateRate>10</updateRate>
+ <controller:gazebo_ros_bumper name="elevator_button2_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>10</updateRate>
+ <frameName>elevator_button2</frameName>
+ <bumperTopicName>elevator_button2_bumper</bumperTopicName>
+ <interface:bumper name="elevator_button2_bumper_iface" />
+ </controller:gazebo_ros_bumper>
+ </sensor:contact>
+ <material>Gazebo/Red</material>
+ <turnGravityOff>false</turnGravityOff>
+ </gazebo>
+
+
+ <!-- elevator 1 -->
+ <link name="elevator_left">
+ <inertial>
+ <origin xyz="0 0 0" />
+ <mass value="5.0" />
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="0.04 0.87 ${size_z_wall1}" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="0.04 0.87 ${size_z_wall1}" />
+ </geometry>
+ </collision>
+ </link>
+
+ <gazebo reference="elevator_left">
+ <material>IPA/Wall2</material>
+ </gazebo>
+
+ <joint name="joint_elevator_left" type="prismatic">
+ <axis xyz = "0 1 0"/>
+ <origin xyz="${-size_total_x/2+dist_axis_x-wall_depth-4.6-1.6} ${-0.68-dist_axis_y-1.68} ${size_z_wall1/2}" rpy="0 0 ${-dist_axis_yall-0.7853}" />
+ <parent link="dummy_link" />
+ <child link="elevator_left" />
+
+
+ <calibration rising="0.0"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ <limit effort="100" velocity="1.0" lower="0" upper="0.87" />
+ <safety_controller k_velocity="10"/>
+ </joint>
+
+ <transmission name="elevator_left_trans" type="pr2_mechanism_model/SimpleTransmission">
+ <actuator name="elevator_left_motor"/>
+ <joint name="elevator_left_joint"/>
+ <mechanicalReduction>1</mechanicalReduction>
+ </transmission>
+
+
+ <!-- wall ext5 -->
+ <link name="wall_ext5">
+ <inertial>
+ <origin xyz="0 0 0" />
+ <mass value="5.0" />
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="2.4 0.04 ${size_z_wall1}" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="2.4 0.04 ${size_z_wall1}" />
+ </geometry>
+ </collision>
+ </link>
+
+ <gazebo reference="wall_ext5">
+ <material>IPA/WallApartment</material>
+ </gazebo>
+
+ <joint name="joint_wall_ext5" type="fixed">
+ <origin xyz="${-size_total_x/2+dist_axis_x-wall_depth-5.1} ${-0.68-dist_axis_y-0.65-1.5} ${size_z_wall1/2}" rpy="0 0 ${-dist_axis_yall-0.7853}" />
+ <parent link="dummy_link" />
+ <child link="wall_ext5" />
+ </joint>
+
+ <!-- wall ext6 -->
+ <link name="wall_ext6">
+ <inertial>
+ <origin xyz="0 0 0" />
+ <mass value="5.0" />
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="2.4 0.04 ${size_z_wall1}" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="2.4 0.04 ${size_z_wall1}" />
+ </geometry>
+ </collision>
+ </link>
+
+ <gazebo reference="wall_ext6">
+ <material>IPA/WallApartment</material>
+ </gazebo>
+
+ <joint name="joint_wall_ext6" type="fixed">
+ <origin xyz="${-size_total_x/2+dist_axis_x-wall_depth-5.1-0.615} ${-0.68-dist_axis_y-0.65-1.5-0.615} ${size_z_wall1/2}" rpy="0 0 ${-dist_axis_yall-0.7853}" />
+ <parent link="dummy_link" />
+ <child link="wall_ext6" />
+ </joint>
+
+ <!-- elevator_left2 -->
+ <link name="elevator_left2">
+ <inertial>
+ <origin xyz="0 0 0" />
+ <mass value="5.0" />
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="0.04 0.87 ${size_z_wall1}" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="0.04 0.87 ${size_z_wall1}" />
+ </geometry>
+ </collision>
+ </link>
+
+ <gazebo reference="elevator_left2">
+ <material>IPA/WallApartment</material>
+ </gazebo>
+
+ <joint name="joint_elevator_left2" type="fixed">
+ <origin xyz="${-size_total_x/2+dist_axis_x-wall_depth-1.6-3} ${-0.68-dist_axis_y-1.68-1.6} ${size_z_wall1/2}" rpy="0 0 ${-dist_axis_yall-0.7853}" />
+ <parent link="dummy_link" />
+ <child link="elevator_left2" />
+ </joint>
+
+ <!-- elevator 2 -->
+ <link name="elevator_right">
+ <inertial>
+ <origin xyz="0 0 0" />
+ <mass value="5.0" />
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="0.04 0.87 ${size_z_wall1}" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="0.04 0.87 ${size_z_wall1}" />
+ </geometry>
+ </collision>
+ </link>
+
+ <gazebo reference="elevator_right">
+ <material>IPA/Wall2</material>
+ </gazebo>
+
+ <joint name="joint_elevator_right" type="prismatic">
+ <axis xyz = "0 -1 0"/>
+ <origin xyz="${-size_total_x/2+dist_axis_x-wall_depth-4.6-1.6-1.15} ${-0.68-dist_axis_y-1.68-1.15} ${size_z_wall1/2}" rpy="0 0 ${-dist_axis_yall-0.7853}" />
+ <parent link="dummy_link" />
+ <child link="elevator_right" />
+
+ <calibration rising="0.0"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ <limit effort="100" velocity="1.0" lower="0" upper="0.87" />
+ <safety_controller k_velocity="10"/>
+ </joint>
+
+ <transmission name="elevator_right_trans" type="pr2_mechanism_model/SimpleTransmission">
+ <actuator name="elevator_right_motor"/>
+ <joint name="elevator_right_joint"/>
+ <mechanicalReduction>1</mechanicalReduction>
+ </transmission>
+
+ <!-- wall ext7 -->
+ <link name="wall_ext7">
+ <inertial>
+ <origin xyz="0 0 0" />
+ <mass value="5.0" />
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="2.4 0.04 ${size_z_wall1}" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="2.4 0.04 ${size_z_wall1}" />
+ </geometry>
+ </collision>
+ </link>
+
+ <gazebo reference="wall_ext7">
+ <material>IPA/WallApartment</material>
+ </gazebo>
+
+ <joint name="joint_wall_ext7" type="fixed">
+ <origin xyz="${-size_total_x/2+dist_axis_x-wall_depth-5.1-1.15} ${-0.68-dist_axis_y-0.65-1.5-1.15} ${size_z_wall1/2}" rpy="0 0 ${-dist_axis_yall-0.7853}" />
+ <parent link="dummy_link" />
+ <child link="wall_ext7" />
+ </joint>
+
+ <!-- wall ext8 -->
+ <link name="wall_ext8">
+ <inertial>
+ <origin xyz="0 0 0" />
+ <mass value="5.0" />
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="2.4 0.04 ${size_z_wall1}" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="2.4 0.04 ${size_z_wall1}" />
+ </geometry>
+ </collision>
+ </link>
+
+ <gazebo reference="wall_ext8">
+ <material>IPA/WallApartment</material>
+ </gazebo>
+
+ <joint name="joint_wall_ext8" type="fixed">
+ <origin xyz="${-size_total_x/2+dist_axis_x-wall_depth-5.1-1.78} ${-0.68-dist_axis_y-0.65-1.5-1.78} ${size_z_wall1/2}" rpy="0 0 ${-dist_axis_yall-0.7853}" />
+ <parent link="dummy_link" />
+ <child link="wall_ext8" />
+ </joint>
+
+ <!-- elevator 22 -->
+ <link name="elevator_right2">
+ <inertial>
+ <origin xyz="0 0 0" />
+ <mass value="5.0" />
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="0.04 0.87 ${size_z_wall1}" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" />
+ <geometry>
+ <box size="0.04 0.87 ${size_z_wall1}" />
+ </geometry>
+ </collision>
+ </link>
+
+ <gazebo reference="elevator_right2">
+ <material>IPA/WallApartment</material>
+ </gazebo>
+
+ <joint name="joint_elevator_right2" type="fixed">
+ <origin xyz="${-size_total_x/2+dist_axis_x-wall_depth-1.6-1.15-3} ${-0.68-dist_axis_y-1.68-1.15-1.6} ${size_z_wall1/2}" rpy="0 0 ${-dist_axis_yall-0.7853}" />
+ <parent link="dummy_link" />
+ <child link="elevator_right2" />
+ </joint>
<!-- cabinet1 -->
<link name="cabinet1">
<inertial>
View
1  cob_gazebo_worlds/ros/launch/ipa-apartment.launch
@@ -4,5 +4,6 @@
<!-- start gazebo world from .urdf file -->
<include file="$(find cob_gazebo_worlds)/ros/launch/world_urdf.launch" />
+ <node name="elevator" pkg="cob_gazebo_worlds" type="elevator.py" respawn="false" output="screen" />
</launch>
View
70 cob_gazebo_worlds/ros/src/elevator.py
@@ -0,0 +1,70 @@
+#!/usr/bin/python
+
+import time
+import sys
+import roslib
+roslib.load_manifest('cob_gazebo_worlds')
+import rospy
+import random
+from math import *
+
+from gazebo.srv import *
+from gazebo_msgs.srv import *
+from gazebo_msgs.msg import *
+
+
+apply_effort_service = rospy.ServiceProxy('/gazebo/apply_joint_effort', ApplyJointEffort)
+door_closed = True
+
+
+
+def callback(ContactsState):
+
+ if door_closed:
+ if (ContactsState.states != []):
+ rospy.loginfo("button pressed")
+ rand = (random.randint(0,1))
+ if rand == 0:
+ move_door("left")
+ else:
+ move_door("right")
+ else:
+ rospy.logdebug("button not pressed")
+ else:
+ rospy.loginfo("Door Opened")
+
+
+
+def listener():
+
+ rospy.init_node('listener', anonymous=True)
+ rospy.Subscriber("/elevator_button1_bumper/state", ContactsState, callback, queue_size=1)
+ rospy.spin()
+
+def move_door(side):
+
+ door_closed = False
+ req = ApplyJointEffortRequest()
+ req.joint_name = 'joint_elevator_'+side
+ req.start_time.secs = 0
+ req.duration.secs = -10
+ req.effort = 500
+ rospy.loginfo("door is opening")
+ res = apply_effort_service(req)
+
+ rospy.sleep(10)
+ req.effort = -1000
+ rospy.loginfo("door is closing")
+ res = apply_effort_service(req)
+
+ rospy.sleep(10)
+ req.effort = 500
+ res = apply_effort_service(req)
+ door_closed = True
+
+
+if __name__ == '__main__':
+ listener()
+
+
+
Something went wrong with that request. Please try again.