added arm service and bring up camera

mars_rover/config/mars_rover_control.yaml

@@ -21,20 +21,20 @@ controller_manager:
       type: joint_state_broadcaster/JointStateBroadcaster
 
 
-# arm_joint_trajectory_controller:
-#   ros__parameters:
-#     joints:
-#       - arm_01_joint
-#       - arm_02_joint
-#       - arm_03_joint
-#       - arm_04_joint
-#       - arm_tools_joint
-#     interface_name: position
-#     command_interfaces:
-#       - position
-#     state_interfaces:
-#       - position
-#       - velocity
+arm_joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - arm_01_joint
+      - arm_02_joint
+      - arm_03_joint
+      - arm_04_joint
+      - arm_tools_joint
+    interface_name: position
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
 
 # mast_joint_trajectory_controller:
 #   ros__parameters:

mars_rover/launch/mars_rover.launch.py

@@ -99,11 +99,11 @@ def generate_launch_description():
         output='screen'
     )
 
-    # load_arm_joint_traj_controller = ExecuteProcess(
-    #     cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
-    #          'arm_joint_trajectory_controller'],
-    #     output='screen'
-    # )
+    load_arm_joint_traj_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
+             'arm_joint_trajectory_controller'],
+        output='screen'
+    )
 
     # load_mast_joint_traj_controller = ExecuteProcess(
     #     cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
@@ -157,6 +157,7 @@ def generate_launch_description():
             OnProcessExit(
                 target_action=set_hardware_interface_active,
                 on_exit=[load_joint_state_broadcaster,
+                        load_arm_joint_traj_controller,
                         load_wheel_joint_traj_controller,
                         load_steer_joint_traj_controller,
                         load_suspension_joint_traj_controller],

mars_rover/nodes/move_arm

@@ -6,35 +6,50 @@ from builtin_interfaces.msg import Duration
 
 from std_msgs.msg import String, Float64
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from std_srvs.srv import Empty
 
 class MoveArm(Node):
 
     def __init__(self):
         super().__init__('arm_node')
         self.arm_publisher_ = self.create_publisher(JointTrajectory, '/arm_joint_trajectory_controller/joint_trajectory', 10)
-        timer_period = 0.5  # seconds
-        self.timer = self.create_timer(timer_period, self.timer_callback)
+        self.pick_srv = self.create_service(Empty, 'move_arm', self.move_arm_callback)
 
 
-    def timer_callback(self):
+        # timer_period = 20.0  # seconds
+        # self.timer = self.create_timer(timer_period, self.timer_callback)
+
+    def move_arm_callback(self, request, response):
         traj = JointTrajectory()
         traj.joint_names = ["arm_01_joint", "arm_02_joint", "arm_03_joint", "arm_04_joint", "arm_tools_joint"]
 
-        point = JointTrajectoryPoint()
-        point.positions = [0.0,0.0,0.0,1.0,0.0]
-        point.time_from_start = Duration(sec=1)
+        point1 = JointTrajectoryPoint()
+        point1.positions = [0.0,0.0,1.0,0.0,0.0]
+        point1.time_from_start = Duration(sec=4)
+
+        traj.points.append(point1)
+
+        point2 = JointTrajectoryPoint()
+        point2.positions = [0.0,0.0,0.0,0.0,0.0]
+        point2.time_from_start = Duration(sec=8)
+
+
+        traj.points.append(point2)
+
+        self.get_logger().info(f'Publishing: "{traj.points}"')
 
-        traj.points.append(point)
         self.arm_publisher_.publish(traj)
 
+        return response
+
+
 
 def main(args=None):
     rclpy.init(args=args)
 
     arm_node = MoveArm()
 
     rclpy.spin(arm_node)
-
     # Destroy the node explicitly
     # (optional - otherwise it will be done automatically
     # when the garbage collector destroys the node object)

mars_rover/nodes/move_wheel

@@ -68,7 +68,6 @@ class MoveWheel(Node):
             else:
                 target_steer.data = [-alpha_o, alpha_o, -alpha_i, alpha_i]
 
-        self.get_logger().info(f'Publishing: "{target_steer.data}"')
         self.steer_publisher_.publish(target_steer)
 
 

mars_rover/urdf/curiosity_mars_rover.xacro

@@ -143,4 +143,42 @@
         <parameters>$(find mars_rover)/config/mars_rover_control.yaml</parameters>
         </plugin>
     </gazebo>
+
+
+
+    <gazebo reference="camera_link">
+        <sensor type="camera" name="camera_sensor">
+            <visualize>1</visualize>
+            <update_rate>10.0</update_rate>
+            <camera>
+                <pose>1.0 0 0 0 0 -1.57</pose>
+                <horizontal_fov>1.3962634</horizontal_fov>
+                <image>
+                    <width>800</width>
+                    <height>800</height>
+                    <format>R8G8B8</format>
+                </image>
+                <clip>
+                <near>0.01</near>
+                <far>100</far>
+                </clip>
+                <noise>
+                <type>gaussian</type>
+                <stddev>0.007</stddev>
+                </noise>
+            </camera>
+            <always_on>1</always_on>
+            <topic>image_raw</topic>
+            <plugin
+                filename="ignition-gazebo-sensors-system"
+                name="ignition::gazebo::systems::Sensors">
+                <render_engine>ogre2</render_engine>
+                <background_color>1 1 1 1</background_color>
+            </plugin>
+        </sensor>
+    </gazebo>
+
+
+
+
 </robot>

mars_rover/urdf/sensor_mast.xacro

@@ -265,7 +265,7 @@
     <joint name="head_camera_joint" type="fixed">
         <parent link="mast_cameras"/>
         <child link="camera_link"/>
-        <origin xyz="0.13642 -0.218683 0.222314" rpy="0 0 ${PI}"/>
+        <origin xyz="0.13642 -0.218683 0.222314" rpy="0 0 ${-PI/2}"/>
     </joint>
 
   </xacro:macro>

mars_rover/worlds/mars_curiosity.world

@@ -1,5 +1,41 @@
 <sdf version="1.5">
   <world name="default">
+    <gui fullscreen="0">
+
+      <plugin filename="GzScene3D" name="3D View">
+        <ignition-gui>
+          <title>3D View</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="string" key="state">docked</property>
+        </ignition-gui>
+        <engine>ogre2</engine>
+        <scene>scene</scene>
+        <background_color>1.0 1.0 1.0</background_color>
+        <camera_pose>-5.0 0.0 -6.0 0.0 0.0 0.0</camera_pose>
+      </plugin>
+
+      <plugin filename="ImageDisplay" name="Image Display">
+        <ignition-gui>
+          <property key="state" type="string">floating</property>
+        </ignition-gui>
+      </plugin>
+    </gui>
+
+    <server_config>
+    <plugins>
+        <plugin entity_name="*"
+                entity_type="world"
+                filename="ignition-gazebo-scene-broadcaster-system"
+                name="ignition::gazebo::systems::SceneBroadcaster">
+        </plugin>
+        <plugin entity_name="*"
+                entity_type="world"
+                filename="ignition-gazebo-sensors-system"
+                name="ignition::gazebo::systems::Sensors">
+        <render_engine>ogre</render_engine>
+        </plugin>
+    </plugins>
+    </server_config>
 
     <!-- Martian Gravity-->
     <gravity>0 0 -3.711</gravity>