def talker():
# pub = rospy.Publisher('/gazebo/apply_joint_effort', ApplyJointEffort, queue_size=10)
rospy.init_node('dd_ctrl', anonymous=True)
pub = rospy.ServiceProxy('/gazebo/apply_joint_effort',ApplyJointEffort)
rate = rospy.Rate(10) # 10hz
i = 0
buff = ApplyJointEffort()
buff.joint_name = "dd_robot::left_wheel_hinge"
buff.effort = 1.0
buff.start_time = 0.0
buff.duration = 0.0
start_time = rospy.Time(0,0)
end_time = rospy.Time(0.01,0)
while not rospy.is_shutdown():
if i > 30:
buff.effort = -buff.effort
i = 0
i = i + 1
# ['joint_name', 'effort', 'start_time', 'duration']
#pub( buff.joint_name, buff.effort, buff.start_time, buff.duration )
#pub( buff.joint_name, buff.effort, 0.0, 0.1 )
buff.joint_name = "dd_robot::left_wheel_hinge"
pub(buff.joint_name, buff.effort, start_time, end_time)
buff.joint_name = "dd_robot::right_wheel_hinge"
pub(buff.joint_name, buff.effort, start_time, end_time)
rospy.loginfo(buff)
rate.sleep()
def setLeft(pub, val):
buff = ApplyJointEffort()
buff.effort = val
start_time = rospy.Time(0, 0)
end_time = rospy.Time(0.01, 0)
buff.joint_name = "dd_robot::left_wheel_hinge"
pub(buff.joint_name, buff.effort, start_time, end_time)
def setRotation(pub, val):
start_t = rospy.Time(0, 0)
end_t = rospy.Time(0.01, 0)
buff = ApplyJointEffort()
buff.effort = val
buff.joint_name = 'dd_robot::left_wheel_hinge'
pub(buff.joint_name, -buff.effort, start_t, end_t)
buff.joint_name = 'dd_robot::right_wheel_hinge'
pub(buff.joint_name, buff.effort, start_t, end_t)
def setRot(pub, val):
buff = ApplyJointEffort()
buff.effort = val * 10
start_time = rospy.Time(0, 0)
end_time = rospy.Time(0.01, 0)
buff.joint_name = "cc_robot::left_wheel_hinge"
pub(buff.joint_name, buff.effort, start_time, end_time)
buff.joint_name = "cc_robot::right_wheel_hinge"
pub(buff.joint_name, -buff.effort, start_time, end_time)
def setRot(pub, val, direction):
buff = ApplyJointEffort()
buff.effort = val
start_time = rospy.Time(0, 0)
end_time = rospy.Time(0.01, 0)
if direction == "l":
buff.joint_name = "dd_robot::left_wheel_hinge"
pub(buff.joint_name, buff.effort, start_time, end_time)
if direction == "r":
buff.joint_name = "dd_robot::right_wheel_hinge"
pub(buff.joint_name, -buff.effort, start_time, end_time)
def move():
rospy.init_node('swing')
value = ApplyJointEffort()
count = 0
while not rospy.is_shutdown():
value.joint_name = 'joint1'
if count % 2 == 0:
value.effort = 10
value.start_time = rospy.Time(0)
value.duration = rospy.Time(1)
else:
value.effort = 0
value.start_time = rospy.Time(0)
value.duration = rospy.Time(1)
rospy.wait_for_service('/gazebo/apply_joint_effort')
JointEffort = rospy.ServiceProxy('/gazebo/apply_joint_effort',
ApplyJointEffort)
resp1 = JointEffort('joint1', value.effort, value.start_time,
value.duration)
rospy.sleep(2.5)