20240708

mycobot_280/mycobot_280/scripts/follow_display.py

@@ -1,4 +1,5 @@
-#!/usr/bin/env python3
+#!/usr/bin/env python2
+# -*- coding: utf-8 -*-
 import time
 
 import rospy

mycobot_280/mycobot_280/scripts/teleop_keyboard.py

@@ -1,4 +1,6 @@
-#!/usr/bin/env python3
+#!/usr/bin/env python2
+# -*- coding: utf-8 -*-
+
 from __future__ import print_function
 from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus, PumpStatus
 import rospy
@@ -92,11 +94,13 @@ def teleop_keyboard():
 
     # rsp = set_angles(*init_pose)
 
-    while True:
-        res = get_coords()
-        if res.x > 1:
-            break
-        time.sleep(0.1)
+    # while True:
+    #     res = get_coords()
+    #     if res.x > 1:
+    #         break
+    #     time.sleep(0.1)
+
+    res = get_coords()
 
     record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
     print(record_coords)

mycobot_280/mycobot_280_gazebo_moveit/launch/mycobot_follow_gazebo.launch

@@ -0,0 +1,25 @@
+<launch>
+	<arg name="paused" default="false"/>
+    <arg name="gazebo_gui" default="true"/>
+	<!-- Load URDF file,加载URDF文件 -->
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_280_m5_gazebo.urdf"/>
+	<!-- <arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" /> -->
+
+	<include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="world_name" default="worlds/empty.world"/>
+        <arg name="paused" value="$(arg paused)"/>
+        <arg name="gui" value="$(arg gazebo_gui)"/>
+    </include>
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+	<!-- Combinejoin values to TF ，将值合并到TF-->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" >
+		<rosparam param="source_list" subst_value="true">["joint_states"]</rosparam>
+	</node>
+	<!-- show in Rviza,显示在Rviz -->
+	<!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" /> -->
+	<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -x 0 -y 0 -z 1"
+        respawn="false" output="screen" />
+
+    <include file="$(find mycobot_280_gazebo_moveit)/launch/ros_controllers.launch"/>
+</launch>

mycobot_280/mycobot_280_gazebo_moveit/launch/teleop_keyboard_gazebo.launch

@@ -0,0 +1,23 @@
+<launch>
+    <arg name="paused" default="false"/>
+    <arg name="gazebo_gui" default="true"/>
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_280_m5_gazebo.urdf"/>
+
+    <!-- Launch Gazebo -->
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="world_name" default="worlds/empty.world"/>
+        <arg name="paused" value="$(arg paused)"/>
+        <arg name="gui" value="$(arg gazebo_gui)"/>
+    </include>
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF ，将值合并到TF-->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+
+    <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -x 0 -y 0 -z 1"
+        respawn="false" output="screen" />
+
+    <include file="$(find mycobot_280_gazebo_moveit)/launch/ros_controllers.launch"/>
+
+</launch>

mycobot_280/mycobot_280_gazebo_moveit/scripts/follow_display_gazebo.py

@@ -0,0 +1,76 @@
+#!/usr/bin/env python2
+# -*- coding: utf-8 -*-
+import time
+import rospy
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from pymycobot.mycobot import MyCobot
+
+# Global Variables
+joint_names = [
+    "joint2_to_joint1",
+    "joint3_to_joint2",
+    "joint4_to_joint3",
+    "joint5_to_joint4",
+    "joint6_to_joint5",
+    "joint6output_to_joint6",
+]
+
+def talker():
+    rospy.init_node("display", anonymous=True)
+
+    print("Try connect real mycobot...")
+    port = rospy.get_param("~port", "/dev/ttyUSB0")
+    baud = rospy.get_param("~baud", 115200)
+    print("port: {}, baud: {}\n".format(port, baud))
+    try:
+        mycobot = MyCobot(port, baud)
+    except Exception as e:
+        print(e)
+        print(
+            """\
+            \rTry connect mycobot failed!
+            \rPlease check wether connected with mycobot.
+            \rPlease chckt wether the port or baud is right.
+        """
+        )
+        exit(1)
+    mycobot.release_all_servos()
+    time.sleep(0.1)
+    print("Rlease all servos over.\n")
+
+    pub_command = rospy.Publisher("/mycobot_position_controller/command", JointTrajectory, queue_size=10)
+    rate = rospy.Rate(30)  # 30hz
+
+    # pub joint state
+
+    print("publishing ...")
+    while not rospy.is_shutdown():
+
+        angles = mycobot.get_radians()
+        data_list = []
+        for index, value in enumerate(angles):
+            data_list.append(value)
+
+        # rospy.loginfo('{}'.format(data_list))
+
+        # Create JointTrajectory message
+        traj_msg = JointTrajectory()
+        traj_msg.header.stamp = rospy.Time.now()
+        traj_msg.joint_names = joint_names
+
+        point = JointTrajectoryPoint()
+        point.positions = data_list
+        point.velocities = [0.0] * len(joint_names)
+        point.time_from_start = rospy.Duration(0.1)  # Set a small duration to continuously update
+
+        traj_msg.points.append(point)
+        pub_command.publish(traj_msg)
+
+        rate.sleep()
+
+
+if __name__ == "__main__":
+    try:
+        talker()
+    except rospy.ROSInterruptException:
+        pass